code:emulator
This is an old revision of the document!
Table of Contents
C code to simulate NMEA output from a GPS in real time - with live Google Erath Display
The code here perfoms the following: GPSgen.c - converts source balloon track KML files into a 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 similate real time gps output - at the same time emulate creates a realtime 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 realtime display of the kml file generated by emulate (open outer.kml file from google earth to initiate the realtime 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 sorce KML can be from a previous balloon flight - the output of a balloon trajectory forecast - or can be hand crafted to test something specific.
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 realtime 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):
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 = fscanf(stdin,"%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 = fscanf(stdin,"%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 = fscanf(stdin,"%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 - 1, 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.1200782919.txt.gz · Last modified: 2008/07/19 23:31 (external edit)