From 196c75b1d755a63d1bc31c2811fd657e1ca5056f Mon Sep 17 00:00:00 2001
From: OSQZSS <osqzss@gmail.com>
Date: Wed, 26 Aug 2015 17:02:52 +0900
Subject: [PATCH] Add NMEA GGA option

---
 gpssim.c | 115 ++++++++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 106 insertions(+), 9 deletions(-)

diff --git a/gpssim.c b/gpssim.c
index 2add7f1..fa87eff 100644
--- a/gpssim.c
+++ b/gpssim.c
@@ -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);