Code:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/file.h>
#include <signal.h>
#include <gps.h>
/**
Used python code by TJ and Jaroslaw Zachwieja as model to create KML (esp: range/tilt values)
wrote this to use GPSD server to handle low level GPS/serialport work
I used GPSD v3.9, libgps.so was linked to libgps.so.20.0.0 in /usr/local/lib
to build: cc -o kmlgps -L/usr/local/lib -lgps kmlgps.c
**/
static struct gps_data_t GPS;
static struct gps_data_t *gps=&GPS;
static char *ofn="/opt/google/earth/realtime/Realtime_GPS.kml";
static char *outfile = NULL;
void gps_handler(struct gps_data_t *sent, char *buf, size_t len, int level)
{
printf("GPSD: %s\n", buf);
fflush(stdout);
}
void dump_state(struct gps_data_t *collect)
{
float latitude_in = 0.0;
float longitude_in = 0.0;
float speed_in = 0.0;
float heading_in = 0.0;
float altitude_in = 0.0;
int range = 1000;
int tilt = 30;
// check if data is a valid fix
if (((collect->set & STATUS_SET) && collect->status>0) &&
((collect->set & MODE_SET) && collect->fix.mode>1))
{
if (collect->set & LATLON_SET) {
latitude_in=collect->fix.latitude;
longitude_in=collect->fix.longitude;
}
if (collect->set & ALTITUDE_SET)
altitude_in=collect->fix.altitude;
if (collect->set & SPEED_SET)
speed_in=collect->fix.speed;
if (collect->set & TRACK_SET)
heading_in=collect->fix.track;
}
// from python code, they didn't explain the math. I just copied it
int speed = (int)(speed_in * 1.852);
range = ( ( speed / 100 ) * 350 ) + 650;
tilt = ( ( speed / 120 ) * 43 ) + 30;
int heading = (int)heading_in;
if (speed < 10) {
range = 200;
tilt = 30;
heading = 0;
}
if (outfile==NULL) outfile=ofn;
int fd = open(outfile, O_WRONLY | O_CREAT , 0640);
if (fd < 0) {
perror("open");
exit(3);
}
FILE *fp = fdopen(fd, "w");
if (fp==NULL) {
perror("fdopen failed");
exit(4);
}
rewind(fp);
char buf[4096];
sprintf(buf,"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<kml xmlns=\"http://earth.google.com/kml/2.0\">\n<Placemark>\n<name>%d km/h</name>\n<description>GPSD placemark</description>\n<LookAt>\n<longitude>%.6f</longitude>\n<latitude>%.6f</latitude>\n<range>%d</range>\n<tilt>%d</tilt>\n<heading>%d</heading>\n</LookAt>\n<Point>\n<coordinates>%.6f,%.6f,%.2f</coordinates>\n</Point>\n</Placemark>\n</kml>\n",speed,longitude_in,latitude_in,range,tilt,heading,longitude_in,latitude_in,altitude_in);
fwrite(buf, strlen(buf), 1, fp);
fputc('\n', fp);
fclose(fp);
}
void initfile() {
if (outfile==NULL) outfile=ofn;
int fd = open(outfile, O_WRONLY | O_CREAT , 0640);
if (fd < 0) {
perror("open");
exit(3);
}
FILE *fp = fdopen(fd, "w");
if (fp==NULL) {
perror("fdopen failed");
exit(4);
}
rewind(fp);
char buf[1024];
sprintf(buf,"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<kml xmlns=\"http://earth.google.com/kml/2.2\">\n<NetworkLink>\n<name>Realtime GPS</name>\n<open>1</open>\n<Link>\n<href>%s</href>\n<refreshMode>onInterval</refreshMode>\n</Link>\n</NetworkLink>\n</kml>\n", outfile);
fwrite(buf, strlen(buf), 1, fp);
fputc('\n', fp);
fclose(fp);
printf("open this file (%s) with google-earth, it will be listed under Temporary places, add it my places, then create a network link to this file, you only have to do this once", outfile);
exit(0);
}
void usage(char *progname) {
printf("%s [-s machinename] [-p netport] [-w wait] [-f filename] [-i]\n", progname);
printf("\t-f filename\tThe full path to KML file (default /opt/google/earth/realtime/Realtime_GPS.kml)\n");
printf("\t-s machinename\tThe GPSD server IP address (default 127.0.0.1)\n");
printf("\t-p netport\tSpecifiy GPSD port# (default 2947)\n");
printf("\t-w waittime\tPoll gps data period in msec (default 5000)\n");
printf("\t-i initialize flag\tCreate init file, must link via google-earth\n");
}
void
termination_handler (int signum)
{
fprintf(stderr,"Signal %d received, kmlgps terminating\n", signum);
(void) gps_stream(gps, WATCH_DISABLE, NULL);
(void) gps_close (gps);
exit(0);
}
int main(int argc, char *argv[])
{
char *server="127.0.0.1";
char *port="2947";
int wait = 5000; // 5 sec
int c;
extern char *optarg;
int initflag=0;
while ( (c=getopt(argc,argv,"i?f:p:s:w:")) != EOF )
switch (c) {
case 'i':
initflag=1;
break;
case 'f':
outfile=optarg;
break;
case 's':
server=optarg;
break;
case 'p':
port=optarg;
break;
case 'w':
wait=atoi(optarg);
break;
case '?':
usage(argv[0]);
exit(1);
}
if (initflag>0) initfile();
if (signal (SIGINT, termination_handler) == SIG_IGN)
signal (SIGINT, SIG_IGN);
if (signal (SIGHUP, termination_handler) == SIG_IGN)
signal (SIGHUP, SIG_IGN);
if (signal (SIGTERM, termination_handler) == SIG_IGN)
signal (SIGTERM, SIG_IGN);
memset(gps,0,sizeof(GPS));
//printf("gps_data_t size %d\n",sizeof(GPS)); // is 6920 for me
int fd = gps_open(server, port, gps);
if(fd<0)
{
perror("gpsd server not found");
exit(1);
}
(void) gps_stream(gps, WATCH_ENABLE | WATCH_JSON, NULL);
while(1) {
if (gps_waiting (gps, wait)) { //in msecs
if (gps_read (gps) == -1) {
perror("gps_read");
} else {
dump_state(gps);
}
}
}
return 0;
}
Adv Reply
Bookmarks