===== C code to simulate NMEA output from a GPS in real time - with live Google Earth Display ===== The code here performs the following: **GPSgen.c** - converts source balloon track KML files into GPS NEMA logs (i.e. files containing $GPGGA messages etc.) **emulate.c** - outputs a GPS log to the PC serial port - pacing the output to simulate real time gps output - at the same time //emulate// creates a real-time KML file so you can see where the payload is currently flying using Google Earth. **outer.kml** is a kml shell that is configured to initiate the real-time display of the kml file generated by emulate (open //outer.kml// file from Google Earth to initiate the real-time display). Together these programs allow real time testing of payloads with a GPS simulation - as if the payload was flying through the coordinates in the source KML. This allows cut-down algorithms etc. to be checked. The source KML can be from a previous balloon flight - the output of a balloon trajectory forecast - or can be hand crafted to test something specific. Some test NMEA files can be found [[code:NMEA|here]] The screenshot below shows the emulation of the Pegasus 1 flight running in the top left DOS window, the generated NMEA in the bottom left DOS window against a background of a Google Earth real-time display of the flight. {{code:scrsh75.jpg|}} Both programs are "filters" (a program that reads form standard input and writes to standard output)- here is how to use them on the command line in a DOS environment (unix is similar): GPSgen gps.log emulate COM2: The dos "mode" command can be used to change the serial port speed. e.g. mode COM2:9600,N,8,1 Both programs are written using stanard POSIX function calls - so should be portble to many environments including unix and DOS. ====== GPSgen.c ====== // Read KML on standard in // Output GPS simulation to standard out // Assume:- // Ascent rate is linear (5.0 m/sec) // Descent rate is Logarithmic dependant on height (5.0 m/sec at ground level) // // // The algorithm works as follows: // read a pair of co-ordinates from the KML (each Longitude, Latitude, Altitude) // the first is deemed to be the "from" co-ordinate the next the "to" coordinate // if the "to" coordinate is above the "from" co-ordinate then the flight is deemed to be Ascending // if the if the "to" coordinate is below the "from" coordinate then the flight is deemed to be Descending // Estimate the time taken to fly between the two coordinates from the geometric // mean of velocities at "from" and "to" altitudes. // using linear interpolation calculate the position and altitude in 1 second steps between "from" and "to" // based on the geometric mean velocity. // // When the "to" coordinate is reached :- // the "to" coordinate is moved to the "from" co-ordinate // the next KML co-ordinate is fetched into the "to" co-ordinate // the process continues until the current position reaches the last coordinate in the KML // // Course and direction are calculated between the "from" and "to" co-ordinates and apply // to all samples between the points. // #include #include #include #include #include // radians to degrees #define DEGREES(x) ((x) * 57.295779513082320877) // degrees to radians #define RADIANS(x) ((x) / 57.295779513082320877) #define LOG_BASE 1.4142135623730950488 #define LOG_POWER 5300.0 time_t Now; // the time of starting this program char buf[128]; // calculate a CRC for the line of input void do_crc(char *pch) { unsigned char crc; if (*pch != '$') return; // does not start with '$' - so can't CRC pch++; // skip '$' crc = 0; // scan between '$' and '*' (or until CR LF or EOL) while ((*pch != '*') && (*pch != '\0') && (*pch != '\r') && (*pch != '\n')) { // checksum calcualtion done over characters between '$' and '*' crc ^= *pch; pch++; } // add or re-write checksum sprintf(pch,"*%02X\r\n",(unsigned int)crc); } // Speed in Kph // Course over ground relative to North // // In normal operation the GPS emulated sends the following sequence of messages // $GPGGA, $GPRMC, $GPVTG // $GPGGA, $GPGSA, $GPRMC, $GPVTG // $GPGGA, $GPGSV ... $GPGSV, $GPRMC, $GPVTG // Output_NEMA - Output the position in NMEA // Latitude & Longtitude in degrees, Altitude in meters Speed in meters/sec void Output_NEMA(time_t Time, double Lat, double Lon, double Alt, double Course, double Speed) { int LatDeg; // latitude - degree part double LatMin; // latitude - minute part char LatDir; // latitude - direction N/S int LonDeg; // longtitude - degree part double LonMin; // longtitude - minute part char LonDir; // longtitude - direction E/W struct tm *ptm; ptm = gmtime(&Time); if (Lat >= 0) LatDir = 'N'; else LatDir = 'S'; Lat = fabs(Lat); LatDeg = (int)(Lat); LatMin = (Lat - (double)LatDeg) * 60.0; if (Lon >= 0) LonDir = 'E'; else LonDir = 'W'; Lon = fabs(Lon); LonDeg = (int)(Lon); LonMin = (Lon - (double)LonDeg) * 60.0; // $GPGGA - 1st in epoc - 5 satellites in view, FixQual = 1, 45m Geoidal separation HDOP = 2.4 sprintf(buf,"$GPGGA,%02d%02d%02d.000,%02d%07.4f,%c,%03d%07.4f,%c,1,05,02.4,%.1f,M,45.0,M,,*", ptm->tm_hour,ptm->tm_min,ptm->tm_sec,LatDeg,LatMin,LatDir,LonDeg,LonMin,LonDir,Alt); do_crc(buf); // add CRC to buf fputs(buf,stdout); switch((int)Time % 3) { // include 'none' or $GPGSA or $GPGSV in 3 second cycle case 0: break; case 1: // 3D fix - 5 satellites (3,7,18,19 & 22) in view. PDOP = 3.3,HDOP = 2.4, VDOP = 2.3 sprintf(buf,"$GPGSA,A,3,03,07,18,19,22,,,,,,,,3.3,2.4,2.3*"); do_crc(buf); // add CRC to buf fputs(buf,stdout); break; case 2: // two lines og GPGSV messages - 1st line of 2, 8 satellites being tracked in total // 03,07 in view 11,12 being tracked sprintf(buf,"$GPGSV,2,1,08,03,89,276,30,07,63,181,22,11,,,,12,,,*"); do_crc(buf); // add CRC to buf fputs(buf,stdout); // GPGSV 2nd line of 2, 8 satellites being tracked in total // 18,19,22 in view 27 being tracked sprintf(buf,"$GPGSV,2.2,08,18,73,111,35,19,33,057,27,22,57,173,37,27,,,*"); do_crc(buf); // add CRC to buf fputs(buf,stdout); break; } //$GPRMC sprintf(buf,"$GPRMC,%02d%02d%02d.000,A,%02d%07.4f,%c,%03d%07.4f,%c,%.2f,%.2f,%02d%02d%02d,,,A*", ptm->tm_hour,ptm->tm_min,ptm->tm_sec,LatDeg,LatMin,LatDir,LonDeg,LonMin,LonDir,Speed * 1.943844,Course,ptm->tm_mday,ptm->tm_mon + 1,ptm->tm_year % 100); do_crc(buf); // add CRC to buf fputs(buf,stdout); // $GPVTG message last in epoc sprintf(buf,"$GPVTG,%.2f,T,,,%.2f,N,%.2f,K,A*",Course,Speed * 1.943844,Speed * 3.6); do_crc(buf); // add CRC to buf fputs(buf,stdout); } // do a KML segment between two coordinates void do_segment(double FromLat, double FromLon, double FromAlt, double ToLat, double ToLon, double ToAlt) { double Rate; // ascent(+ve) / decent(-ve) rate meters per second (for segment) int Duration; // calculated segment duration in seconds double Elapsed; // floating point duration double Lat,Lon,Alt; // clacualted for each step double Course,Speed; // calculated for entire segment double Distance; // distance between coordinates double DeltaLat,DeltaLon,DeltaAlt; // Latitude, Longtitude and Altitude differences double AdjLon; // Adjusted Longtitude difference int i; // counter DeltaAlt = (ToAlt - FromAlt); DeltaLat = (ToLat - FromLat); DeltaLon = (ToLon - FromLon); if (DeltaAlt >= 0) { // ascending Rate = 5.0; } else { // descending - clculate the geometric mean of the expected velocities at To and From altitude Rate = -5.0 * sqrt(pow(LOG_BASE,(FromAlt / LOG_POWER)) * pow(LOG_BASE,(ToAlt / LOG_POWER))); } // calcualte time (secs) between co-ordinates Elapsed = DeltaAlt / Rate; // always positive Duration = (int)Elapsed; if ((Elapsed - (float)Duration) >= 0.5) Duration++; // round duration of segment to nearest integer // Calculate Course (degrees relative to north) for entire segment Course = atan2(DeltaLat,DeltaLon); // result is +PI radians (cw) to -PI radians (ccw) from x (Longtitude) axis Course = DEGREES(Course); // convert radians to degrees if (Course <= 90.0) Course = 90.0 - Course; else Course = 450.0 - Course; // convert to 0 - 360 clockwise from north // Calculate Speed (m/sec) for entire segment AdjLon = cos(RADIANS((FromLat + ToLat) / 2.0)) * DeltaLon; Distance = (sqrt((DeltaLat * DeltaLat) + (AdjLon * AdjLon)) * 111194.9266); // result in meters Speed = Distance / (double)Duration; // meters per second // calculate 1 second "step" sizes DeltaAlt /= (double)Duration; DeltaLat /= (double)Duration; DeltaLon /= (double)Duration; // now output the NMEA for each step between From and To (but excluding To - which is picked up on next segment) Lat = FromLat; Lon = FromLon; Alt = FromAlt; for (i = 0; i < Duration; i++) { Output_NEMA(Now,Lat,Lon,Alt,Course,Speed); Now++; // 1 second steps Lat += DeltaLat; Lon += DeltaLon; Alt += DeltaAlt; } } // look for a particular string in the input // error if not found in input // void look_for(char *string) { int i; while(1) { i = scanf("%128s",buf); // read a non whitespace from the input if (i != 1) { fprintf(stderr,"No %s found\n",string); exit(1); // abnormal termination } if (strcmp(buf,string) == 0) { return; // string found } } } // Read in .KML file (extract co-ordinate part) // Make assumptions about Ascent and Decent Rates // interpolate positions // Output GPS in pseudo real time int main(int argc, char **argv) { int i; float FromLon,FromLat,FromAlt, ToLon,ToLat,ToAlt; look_for(""); // look for 1st token look_for(""); // look for subsiquent token FromLon = FromLat = FromAlt = ToLon = ToLat = ToAlt = 0.0; // get first LineString co-ordinate as launch position i = scanf("%f , %f , %f",&FromLon,&FromLat,&FromAlt); if (i !=3) { fprintf(stderr,"1st coordinate not 3D\n"); return 1; // abnormal termination } Now = time(NULL); // use the current time as a reference // get subsiquent LineString co-ordinates while(1) { i = scanf("%f , %f , %f",&ToLon,&ToLat,&ToAlt); if (i != 3) break; // not co-ordinate do_segment(FromLat,FromLon,FromAlt,ToLat,ToLon,ToAlt); FromLon = ToLon; FromLat = ToLat; FromAlt = ToAlt; } Output_NEMA(Now,ToLat,ToLon,ToAlt,0.0,0.0); // Final Position (assume stationary) look_for(""); // look for closing token look_for(""); // look for closing token return 0; } ====== Emulate.c ====== // emulate.c - a program to emulate NMEA sentances from a GPS // it expects to be given an input file similar to a GPS log. // it calculates and adds NMEA checksums and paces the output as if it were being sent in real time. // // emulate is a unix 'filter' i.e. it reads from standard input and write to standard output // any arguments are ignored // // use with command line re-direction to output to serial port // dos e.g. emulate COM2: // to send gps.log to the COM2 serial port (adding checksums and re-timing on the way) // // the dos "mode" command can be used to change the serial port speed. // e.g. mode COM2:9600,N,8,1 // to set to 9600baud, No parity,8 data bits, 1 stop // #include /* Standard input/output definitions */ #include /* Standard stuff like exit */ #include #include /* String function definitions */ #include // radians to degrees #define DEGREES(x) ((x) * 57.295779513082320877) // degrees to radians #define RADIANS(x) ((x) / 57.295779513082320877) #define BUF_SIZE 256 // size of input buffer #define NONE 0 #define GPGGA 1 #define GPRMC 2 #define GPVTG 3 char buf[BUF_SIZE]; // input buffer double BaseSec = 0.0; // set to time of first valid reading in file double BaseLat = 0.0; // set to Latitude of first valid reading in file double BaseLon = 0.0; // set to Longtitude time of first valid reading in file // ************************************************************************************** // // This routines constructs the "dynamic" KML file // // // lat,lon,alt and description set for modes 1 and 2 // first call (kml_state == 0) causes jus the file header etc to be written // the 2nd call (kml_state == 1) writes the launch position // subsiquent calls (kml_state == 2) write the current position long lastpos; FILE *fpkml; int kml_state = 0; char *kml_file = "livekml.kml"; void kml_gen(double lat, double lon, double alt, char * description) { switch(kml_state) { case 0: // this section written on startup if((fpkml = fopen(kml_file,"w")) == NULL) // create file - wipe existing contens return; // cant open it now - maybe next-time fprintf(fpkml,"\n"); fprintf(fpkml,"\n"); fprintf(fpkml,"\n"); fprintf(fpkml,"\n"); fprintf(fpkml,"\n"); lastpos = ftell(fpkml); // rewind to here to add launch placemark and initial position kml_state++; // state 1 next time break; case 1: // this section written when initial position placemark when it is known if((fpkml = fopen(kml_file,"r+")) == NULL) // open exiting file return; // cant open it now - maybe next-time fseek(fpkml,lastpos,SEEK_SET); // rewind to write over final section of state 0 call fprintf(fpkml," Launch #place\n"); fprintf(fpkml,"%s\n",description); fprintf(fpkml," absolute\n"); fprintf(fpkml,"%f,%f,%f\n",lon,lat,alt); fprintf(fpkml,"\n\n"); fprintf(fpkml," Flight Path #track\n"); fprintf(fpkml," 1 absolute\n"); fprintf(fpkml,"\n"); fprintf(fpkml,"%f,%f,%f\n",lon,lat,alt); lastpos = ftell(fpkml); // rewind to here to add next co-ordinate fprintf(fpkml,"\n\n\n"); // gets overwritten in subsiquent calls kml_state++; // state 2 next time break; case 2: // this section writes an additional co-ordinate to the file and current position placemark if((fpkml = fopen(kml_file,"r+")) == NULL) // open exiting file return; // cant open it now - maybe next-time fseek(fpkml,lastpos,SEEK_SET); fprintf(fpkml,"%f,%f,%f\n",lon,lat,alt); lastpos = ftell(fpkml); // rewind to here to add next co-ordinate fprintf(fpkml,"\n\n\n"); // gets overwritten in subsiquent calls fprintf(fpkml," Position Now #place\n"); fprintf(fpkml,"%s\n",description); fprintf(fpkml," absolute\n"); fprintf(fpkml,"%f,%f,%f\n",lon,lat,alt); fprintf(fpkml,"\n\n"); break; // case 2 next time } // this bit always written fprintf(fpkml,"\n"); fprintf(fpkml,"\n"); fclose(fpkml); // ensure file is written to disk } // read each line from the input int read_input_line(char *pch, int size) { do { if (fgets(pch, size, stdin) == NULL) return 0; } while(buf[0] != '$'); // loop until NMEA valid line read return 1; // line read OK } // calculate a CRC for the line of input void re_crc(char *pch) { unsigned char crc; if (*pch != '$') return; // does not start with '$' - so cant CRC pch++; // skip '$' crc = 0; // scan between '$' and '*' (or until CR LF or EOL) while ((*pch != '*') && (*pch != '\0') && (*pch != '\r') && (*pch != '\n')) { // checksum calcualtion done over characters between '$' and '*' crc ^= *pch; pch++; } // re-write (or add checksum) sprintf(pch,"*%02X\r\n",(unsigned int)crc); } // Claculate Course & Distance void Calc_Vector(double FromLat, double FromLon, double ToLat, double ToLon, double *pCourse, double *pDistance) { double DeltaLat, DeltaLon; double Course; DeltaLat = ToLat - FromLat; DeltaLon = ToLon - FromLon; Course = atan2(DeltaLat,DeltaLon); // result is +PI radians (cw) to -PI radians (ccw) from x (Longtitude) axis Course = DEGREES(Course); // convert radians to degrees if (Course <= 90.0) *pCourse = 90.0 - Course; else *pCourse = 450.0 - Course; // convert to 0 - 360 clockwise from north DeltaLon = cos(RADIANS((FromLat + ToLat) / 2.0)) * DeltaLon; // adjust lontitude to equator equivelent *pDistance = (sqrt((DeltaLat * DeltaLat) + (DeltaLon * DeltaLon)) * 111.1949266); // result in Km } // degrees to 16 point compass conversion // returns a pointer to 3 characters (4 if you want the comma) // containing the corresponding compass point to the // input bearing (0 - 360 degrees relative to north) char points[] = " N,NNE, NE,ENE, E,ESE, SE,SSE, S,SSW, SW,WSW, W,WNW, NW,NNW,"; char *deg_to_compass16(double bearing) { int point; point = (int)((bearing + 11.25) / 22.5); // calculate a compas point 0 - 16 for 0 - 360 point &= 0x000F; // wrap 16 to 0 return &points[point * 4]; } // convert Hours, Minutes & Seconds (in minute) to just decimal seconds // double Time_to_Sec(int Hours, int Minutes, double Seconds) { return Seconds + ((double)Minutes * 60.0) + ((double)Hours * 3600.0); } // convert degress, minutes and directiion into signed decimal degrees double DegMin_to_Deg(double Deg, double Min, char Dir) { Deg += (Min /= 60.0); // convert minutes to degrees if ((Dir == 'N') || (Dir == 'E')) return Deg; else return -Deg; // Assume 'W' or 'S' } double next_time = 0.0; int parse_NMEA(char *pch) { int i; int Hour, Minute; double Second; double LatDeg, LonDeg; double LatMin, LonMin; char LatDir, LonDir; int NSats; double HDOP; double Alt; char Val; char FixQual; double SpeedKn; double SpeedKph; double Course; int Day, Month, Year; double Distance; double Bearing; i = sscanf(pch,"$GPGGA,%2d%2d%lf,%2lf%lf,%c,%3lf%lf,%c,%c,%d,%lf,%lf,M,", &Hour,&Minute,&Second,&LatDeg,&LatMin,&LatDir,&LonDeg,&LonMin,&LonDir,&FixQual,&NSats,&HDOP,&Alt); if (i > 0) { // some fileds converted Second = Time_to_Sec(Hour,Minute,Second); // convert to decimal seconds LatDeg = DegMin_to_Deg(LatDeg,LatMin,LatDir); // convert to decimal degrees Latitude LonDeg = DegMin_to_Deg(LonDeg,LonMin,LonDir); // convert to decimal degrees Longtitude if ((Second != 0.0) && (LatDeg != 0.0) && (LonDeg != 0.0)) { // valid position if(BaseSec == 0.0) { BaseSec = Second; // capture first time in file BaseLat = LatDeg; // first Latitude BaseLon = LonDeg; // first Longtitude kml_gen(LatDeg,LonDeg,Alt,"Launch!"); // first positions next_time = Second + 20.0; } else { if (Second >= next_time) { kml_gen(LatDeg,LonDeg,Alt,"HereNoW!"); // subsiquent positions next_time = Second + 20.0; } } } Calc_Vector(BaseLat, BaseLon, LatDeg, LonDeg, &Bearing, &Distance); // calculate bearin & distance fprintf(stderr,"\nTi=%4.0f Po=%.4f,%.4f Al=%5.0f Be=%5.1f %.3s Km=%.1f", Second - BaseSec,LatDeg,LonDeg,Alt,Bearing,deg_to_compass16(Bearing),Distance); return GPGGA; } i = sscanf(pch,"$GPRMC,%2d%2d%lf,%c,%2lf%lf,%c,%3lf%lf,%c,%lf,%lf,%2d%2d%2d,", &Hour,&Minute,&Second,&Val,&LatDeg,&LatMin,&LatDir,&LonDeg,&LonMin,&LonDir,&SpeedKn,&Course,&Day,&Month,&Year); if (i > 0) { // some fields converted fprintf(stderr," Co=%5.1f %.3s Kh=%.1f%",Course,deg_to_compass16(Course),SpeedKn * 1.852); return GPRMC; } i = sscanf(pch,"$GPVTG,%lf,T,,,%llf,N,%f,K*", &Course,&SpeedKn,&SpeedKph); if (i > 0) { // some fileds converted return GPVTG; } return NONE; } // write the line to the output void write_serial_io(char *pch) { fputs(pch,stdout); fflush(stdout); } // ****************************************************************************************** // *************************************** Main ********************************************* // lines are played out in pseudo real time - GPGGA messages are held until 1 second has elapsed since the previous // (re)calcualtes NMEA checksum int main (int argc, char **argv) { time_t now; // set to current time long gga_count; time_t epoch; // time the output started now = epoch = time(NULL); // cpature the start time gga_count = 0l; kml_gen(0.0,0.0,0.0,""); // create KML file etc. (state 0) // the main loop while (read_input_line(buf, sizeof(buf))) // Loop until end of file read - read standard input { re_crc(buf); // re-calculate CRC and add if (parse_NMEA(buf) == GPGGA) // parse input (and do output messages) { // pace $GPGGA messages to one per second gga_count++; while ((now - epoch) < gga_count) now = time(NULL); // wait until elapsed time in seconds catches up with number of gga messages } write_serial_io(buf); // write to standard output } return 0; // normal termination } ====== outer.kml ====== livekml.kml onInterval 10