GPS GNSS
Dependencies: Vector3
Diff: GPS_HAPS.cpp
- Revision:
- 3:b0d594e5d9e2
- Parent:
- 2:abd509e1f4d1
- Child:
- 4:708e05b10257
diff -r abd509e1f4d1 -r b0d594e5d9e2 GPS_HAPS.cpp --- a/GPS_HAPS.cpp Tue Sep 07 06:58:56 2021 +0000 +++ b/GPS_HAPS.cpp Tue Sep 07 08:24:31 2021 +0000 @@ -75,15 +75,35 @@ if(strstr(cmd,"GGA") != NULL) { sscanf(cmd, "%[^,],%f,%f,%c,%f,%c,%d,%d,%f", s, &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); - float lat = (int)latitude/100; - Latitude = lat + (latitude-lat*100)/60.0f; - float lon = (int)longitude/100; - Longitude = lon + (longitude-lon*100)/60.0f; + if (abs(latitude) < ERROR_F) + { + Latitude = -1.0f; + } + else + { + float lat = (int)latitude/100; + Latitude = lat + (latitude-lat*100)/60.0f; + } + if (abs(longitude) < ERROR_F) + { + Longitude = -1.0f; + } + else + { + float lon = (int)longitude/100; + Longitude = lon + (longitude-lon*100)/60.0f; + } N_S = ns; E_W = ew; Quality = fq; Satellites = nst; - Altitude = altitude; + if (abs(altitude) < ERROR_F) + { + Altitude = -1.0f; + } + { + Altitude = altitude; + } // pc.printf("%s\n", cmd); // pc.printf("##GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\r\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude); } @@ -108,18 +128,28 @@ { sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date); // pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date); - float time = timefix + 90000.00f; - Hours = int(time) / 10000; - Minutes = (int(time) % 10000) / 100; - Seconds = int(time) % 100; - Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f); - Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds; - Speed = speed * 0.514444f; - Year = date % 100; - date /= 100; - Month = date % 100; - date /= 100; - Day = date; + if (abs(timefix) > ERROR_F) + { + float time = timefix + 90000.00f; + Hours = int(time) / 10000; + Minutes = (int(time) % 10000) / 100; + Seconds = int(time) % 100; + Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f); + Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds; + if (abs(speed) < ERROR_F) + { + Speed = -1.0f; + } + else + { + Speed = speed * 0.514444f; + } + Year = date % 100; + date /= 100; + Month = date % 100; + date /= 100; + Day = date; + } // pc.printf("%s\n", cmd); // pc.printf("##GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\r\n", timefix, status, latitude, ns, longitude, ew, speed, date); }