diff --git a/tools/nmea2um.c b/tools/nmea2um.c new file mode 100644 index 0000000..924c9d6 --- /dev/null +++ b/tools/nmea2um.c @@ -0,0 +1,139 @@ +// nmea2um: +// Convert NMEA GGA data generated by the free GPS NMEA simulation +// software from LabSat into the ECEF user motion data for gps-sdr-sim. +// http://www.labsat.co.uk/index.php/jp/free-gps-nmea-simulator-software + +#define _CRT_SECURE_NO_DEPRECATE + +#include +#include +#include +#include + +#define MAX_CHAR 256 +#define R2D 57.2957795131 + +void llh2xyz(double *llh, double *xyz) +{ + double n; + double a; + double e; + double e2; + double clat; + double slat; + double clon; + double slon; + double d,nph; + double tmp; + + a = 6378137.0; + e = 0.0818191908426; + e2 = e*e; + + clat = cos(llh[0]); + slat = sin(llh[0]); + clon = cos(llh[1]); + slon = sin(llh[1]); + d = e*slat; + + n = a/sqrt(1.0-d*d); + nph = n + llh[2]; + + tmp = nph*clat; + xyz[0] = tmp*clon; + xyz[1] = tmp*slon; + xyz[2] = ((1.0-e2)*n + llh[2])*slat; + + return; +} + +int main(int argc, char *argv[]) +{ + FILE *inp,*outp; + char str[MAX_CHAR]; + char *token; + double llh[3],xyz[3],t=0.0; + char tmp[8]; + + if (argc!=3) + { + printf("Usage: nmea2um \n"); + exit(1); + } + + if ((inp=fopen(argv[1],"rt"))==NULL) + { + printf("Failed to open NMEA file.\n"); + exit(1); + } + + if ((outp=fopen(argv[2],"wt"))==NULL) + { + printf("Failed to open user motion file.\n"); + exit(1); + } + + while (1) + { + if (fgets(str, MAX_CHAR, inp)==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, xyz); + + // Print out + fprintf(outp, "%5.1f,%12.3f,%12.3f,%12.3f\n", t, xyz[0], xyz[1], xyz[2]); + + t += 0.1; // at 10Hz + } + } + + fclose(inp); + fclose(outp); + + return(0); +} \ No newline at end of file