Add NMEA GGA option

This commit is contained in:
OSQZSS 2015-08-26 17:02:52 +09:00
parent a55476985d
commit 196c75b1d7

115
gpssim.c
View File

@ -1185,6 +1185,7 @@ int readUserMotion(double xyz[USER_MOTION_SIZE][3], const char *filename)
{
FILE *fp;
int numd;
char str[MAX_CHAR];
double t,x,y,z;
if (NULL==(fp=fopen(filename,"rt")))
@ -1192,7 +1193,10 @@ int readUserMotion(double xyz[USER_MOTION_SIZE][3], const char *filename)
for (numd=0; numd<USER_MOTION_SIZE; numd++)
{
if (EOF==fscanf(fp, "%lf,%lf,%lf,%lf", &t, &x, &y, &z)) // Read CSV file
if (fgets(str, MAX_CHAR, fp)==NULL)
break;
if (EOF==sscanf(str, "%lf,%lf,%lf,%lf", &t, &x, &y, &z)) // Read CSV line
break;
xyz[numd][0] = x;
@ -1205,12 +1209,94 @@ int readUserMotion(double xyz[USER_MOTION_SIZE][3], const char *filename)
return (numd);
}
int readNmeaGGA(double xyz[USER_MOTION_SIZE][3], const char *filename)
{
FILE *fp;
int numd = 0;
char str[MAX_CHAR];
char *token;
double llh[3],pos[3];
char tmp[8];
if (NULL==(fp=fopen(filename,"rt")))
return(-1);
while (1)
{
if (fgets(str, MAX_CHAR, fp)==NULL)
break;
token = strtok(str, ",");
if (strncmp(token+3, "GGA", 3)==0)
{
token = strtok(NULL, ","); // Date and time
token = strtok(NULL, ","); // Latitude
strncpy(tmp, token, 2);
tmp[2] = 0;
llh[0] = atof(tmp) + atof(token+2)/60.0;
token = strtok(NULL, ","); // North or south
if (token[0]=='S')
llh[0] *= -1.0;
llh[0] /= R2D; // in radian
token = strtok(NULL, ","); // Longitude
strncpy(tmp, token, 3);
tmp[3] = 0;
llh[1] = atof(tmp) + atof(token+3)/60.0;
token = strtok(NULL, ","); // East or west
if (token[0]=='W')
llh[1] *= -1.0;
llh[1] /= R2D; // in radian
token = strtok(NULL, ","); // GPS fix
token = strtok(NULL, ","); // Number of satellites
token = strtok(NULL, ","); // HDOP
token = strtok(NULL, ","); // Altitude above meas sea level
llh[2] = atof(token);
token = strtok(NULL, ","); // in meter
token = strtok(NULL, ","); // Geoid height above WGS84 ellipsoid
llh[2] += atof(token);
// Convert geodetic position into ECEF coordinates
llh2xyz(llh, pos);
xyz[numd][0] = pos[0];
xyz[numd][1] = pos[1];
xyz[numd][2] = pos[2];
// Update the number of track points
numd++;
if (numd>=USER_MOTION_SIZE)
break;
}
}
fclose(fp);
return (numd);
}
void usage(void)
{
printf("Usage: gps-sdr-sim [options]\n"
"Options:\n"
" -e <gps_nav> RINEX navigation file for GPS ephemerides (required)\n"
" -u <user_motion> User motion file (dynamic mode)\n"
" -g <nmea_gga> NMEA GGA stream (dynamic mode)\n"
" -l <location> Lat,Lon,Hgt (static mode) e.g. 30.286502,120.032669,100\n"
" -o <output> I/Q sampling data file (default: gpssim.bin)\n"
" -s <frequency> Sampling frequency [Hz] (default: 2600000)\n"
@ -1266,9 +1352,11 @@ int main(int argc, char *argv[])
int iumd;
int numd;
char umfile[MAX_CHAR];
bool staticLocationMode = false;
double xyz[USER_MOTION_SIZE][3];
bool staticLocationMode = false;
bool nmeaGGA = false;
char navfile[MAX_CHAR];
char outfile[MAX_CHAR];
@ -1295,7 +1383,7 @@ int main(int argc, char *argv[])
exit(1);
}
while ((result=getopt(argc,argv,"e:u:l:o:s:b:"))!=-1)
while ((result=getopt(argc,argv,"e:u:g:l:o:s:b:"))!=-1)
{
switch (result)
{
@ -1304,6 +1392,11 @@ int main(int argc, char *argv[])
break;
case 'u':
strcpy(umfile, optarg);
nmeaGGA = false;
break;
case 'g':
strcpy(umfile, optarg);
nmeaGGA = true;
break;
case 'l':
// Static geodetic coordinates input mode
@ -1349,8 +1442,8 @@ int main(int argc, char *argv[])
if (umfile[0]==0 && !staticLocationMode)
{
printf("User motion file is not specified.\n");
printf("Or you may use -l to specify llh coordinate directly.\n");
printf("User motion file / NMEA GGA stream is not specified.\n");
printf("You may use -l to specify the static location directly.\n");
exit(1);
}
@ -1368,19 +1461,23 @@ int main(int argc, char *argv[])
if (!staticLocationMode)
{
// Read user motion file
numd = readUserMotion(xyz, umfile);
if (nmeaGGA==true)
numd = readNmeaGGA(xyz, umfile);
else
numd = readUserMotion(xyz, umfile);
if (numd==-1)
{
printf("Failed to open user motion file.\n");
printf("Failed to open user motion / NMEA GGA file.\n");
exit(1);
}
else if (numd==0)
{
printf("Failed to read user motion data.\n");
printf("Failed to read user motion / NMEA GGA data.\n");
exit(1);
}
printf("User motion data = %d\n", numd);
printf("Track points = %d\n", numd);
// Initial location in Geodetic coordinate system
xyz2llh(xyz[0], llh);