code:emulator
Table of Contents
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 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.
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 <file.kml >gps.log
emulate <gps.log >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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>
// 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("<LineString>"); // look for 1st <LineString> token
look_for("<coordinates>"); // look for subsiquent <coordinates> 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("</coordinates>"); // look for closing </coordinates> token
look_for("</LineString>"); // look for closing </LineString> 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 <gps.log >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 <stdio.h> /* Standard input/output definitions */
#include <stdlib.h> /* Standard stuff like exit */
#include <time.h>
#include <string.h> /* String function definitions */
#include <math.h>
// 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,"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
fprintf(fpkml,"<kml xmlns=\"http://earth.google.com/kml/2.1\">\n");
fprintf(fpkml,"<Document>\n");
fprintf(fpkml,"<Style id=\"track\">\n");
fprintf(fpkml,"<LineStyle> <color>fff010c0</color> </LineStyle>\n");
fprintf(fpkml,"<PolyStyle> <color>3fc00880</color> </PolyStyle>\n");
fprintf(fpkml,"</Style>\n");
fprintf(fpkml,"<Style id=\"place\">\n");
fprintf(fpkml,"<IconStyle> <scale>1</scale> <Icon> <href>http://weather.uwyo.edu/icons/purple.gif</href> </Icon> </IconStyle>\n");
fprintf(fpkml,"</Style>\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,"<Placemark> <name>Launch</name> <styleUrl>#place</styleUrl>\n");
fprintf(fpkml,"<description>%s</description>\n",description);
fprintf(fpkml,"<Point> <altitudeMode>absolute</altitudeMode>\n");
fprintf(fpkml,"<coordinates>%f,%f,%f</coordinates>\n",lon,lat,alt);
fprintf(fpkml,"</Point>\n</Placemark>\n");
fprintf(fpkml,"<Placemark> <name>Flight Path</name> <styleUrl>#track</styleUrl>\n");
fprintf(fpkml,"<LineString> <extrude>1</extrude> <altitudeMode>absolute</altitudeMode>\n");
fprintf(fpkml,"<coordinates>\n");
fprintf(fpkml,"%f,%f,%f\n",lon,lat,alt);
lastpos = ftell(fpkml); // rewind to here to add next co-ordinate
fprintf(fpkml,"</coordinates>\n</LineString>\n</Placemark>\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,"</coordinates>\n</LineString>\n</Placemark>\n"); // gets overwritten in subsiquent calls
fprintf(fpkml,"<Placemark> <name>Position Now</name> <styleUrl>#place</styleUrl>\n");
fprintf(fpkml,"<description>%s</description>\n",description);
fprintf(fpkml,"<Point> <altitudeMode>absolute</altitudeMode>\n");
fprintf(fpkml,"<coordinates>%f,%f,%f</coordinates>\n",lon,lat,alt);
fprintf(fpkml,"</Point>\n</Placemark>\n");
break; // case 2 next time
}
// this bit always written
fprintf(fpkml,"</Document>\n");
fprintf(fpkml,"</kml>\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
<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://earth.google.com/kml/2.1">
<Document>
<NetworkLink>
<Link>
<href>livekml.kml</href>
<refreshMode>onInterval</refreshMode>
<refreshInterval>10</refreshInterval>
</Link>
</NetworkLink>
</Document>
</kml>
code/emulator.txt · Last modified: 2021/03/04 14:52 by rocketboy