Parse GPS DOP values (#11912)

This is part 2 of a series to prepare for altitude estimations (and
later on full 3-dimensional estimations) with a new sensor fusion model.

This is a PR to get access to relevant dilution of precision values of
the GPS signal.
- `pDOP`: positional (3D) - for positional dilution in space
- `hDOP`: horizontal (2D) - for positional dilution above ground
- `vDOP`: vertical (1D) - for dilution of altitude

src/main/build/debug.h

@@ -104,6 +104,7 @@ typedef enum {
     DEBUG_GPS_RESCUE_TRACKING,
     DEBUG_ATTITUDE,
     DEBUG_VTX_MSP,
+    DEBUG_GPS_DOP,
     DEBUG_COUNT
 } debugType_e;
 

src/main/flight/position.c

@@ -116,9 +116,9 @@ void calculateEstimatedAltitude(void)
         // On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero.
         gpsAltCm = gpsSol.llh.altCm; // static, so hold last altitude value if 3D fix is lost to prevent fly to moon
         haveGpsAlt = true; // stays false if no 3D fix
-        if (gpsSol.hdop != 0) {
+        if (gpsSol.dop.hdop != 0) {
-            gpsTrust = 100.0f / gpsSol.hdop;
+            gpsTrust = 100.0f / gpsSol.dop.hdop;
-            // *** TO DO - investigate if we should use vDOP or vACC with UBlox units; this hDOP value is actually pDOP in UBLox code !!!
+            // *** TO DO - investigate if we should use vDOP or vACC with UBlox units;
         }
         // always use at least 10% of other sources besides gps if available
         gpsTrust = MIN(gpsTrust, 0.9f);

src/main/io/gps.c

@@ -63,7 +63,9 @@
 #define LOG_IGNORED      '!'
 #define LOG_SKIPPED      '>'
 #define LOG_NMEA_GGA     'g'
+#define LOG_NMEA_GSA     's'
 #define LOG_NMEA_RMC     'r'
+#define LOG_UBLOX_DOP    'D'
 #define LOG_UBLOX_SOL    'O'
 #define LOG_UBLOX_STATUS 'S'
 #define LOG_UBLOX_SVINFO 'I'
@@ -142,6 +144,7 @@ enum {
     MSG_ACK_ACK = 0x01,
     MSG_POSLLH = 0x2,
     MSG_STATUS = 0x3,
+    MSG_DOP = 0x4,
     MSG_SOL = 0x6,
     MSG_PVT = 0x7,
     MSG_VELNED = 0x12,
@@ -890,6 +893,11 @@ void gpsUpdate(timeUs_t currentTimeUs)
         }
     }
 
+    DEBUG_SET(DEBUG_GPS_DOP, 0, gpsSol.numSat);
+    DEBUG_SET(DEBUG_GPS_DOP, 1, gpsSol.dop.pdop);
+    DEBUG_SET(DEBUG_GPS_DOP, 2, gpsSol.dop.hdop);
+    DEBUG_SET(DEBUG_GPS_DOP, 3, gpsSol.dop.vdop);
+
     executeTimeUs = micros() - currentTimeUs;
     if (executeTimeUs > gpsStateDurationUs[gpsCurrentState]) {
         gpsStateDurationUs[gpsCurrentState] = executeTimeUs;
@@ -963,6 +971,7 @@ bool gpsIsHealthy(void)
 #define FRAME_GGA  1
 #define FRAME_RMC  2
 #define FRAME_GSV  3
+#define FRAME_GSA  4
 
 
 // This code is used for parsing NMEA data
@@ -1042,7 +1051,9 @@ typedef struct gpsDataNmea_s {
     uint8_t numSat;
     int32_t altitudeCm;
     uint16_t speed;
+    uint16_t pdop;
     uint16_t hdop;
+    uint16_t vdop;
     uint16_t ground_course;
     uint32_t time;
     uint32_t date;
@@ -1076,6 +1087,8 @@ static bool gpsNewFrameNMEA(char c)
                     gps_frame = FRAME_RMC;
                 } else if (0 == strcmp(string, "GPGSV")) {
                     gps_frame = FRAME_GSV;
+                } else if (0 == strcmp(string, "GPGSA")) {
+                    gps_frame = FRAME_GSA;
                 }
             }
 
@@ -1104,9 +1117,6 @@ static bool gpsNewFrameNMEA(char c)
                         case 7:
                             gps_Msg.numSat = grab_fields(string, 0);
                             break;
-                        case 8:
-                            gps_Msg.hdop = grab_fields(string, 1) * 100;          // hdop
-                            break;
                         case 9:
                             gps_Msg.altitudeCm = grab_fields(string, 1) * 10;     // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10
                             break;
@@ -1177,6 +1187,20 @@ static bool gpsNewFrameNMEA(char c)
                     GPS_svInfoReceivedCount++;
 
                     break;
+
+                case FRAME_GSA:
+                    switch (param) {
+                        case 15:
+                            gps_Msg.pdop = grab_fields(string, 1) * 100;        // pDOP
+                            break;
+                        case 16:
+                            gps_Msg.hdop = grab_fields(string, 1) * 100;        // hDOP
+                            break;
+                        case 17:
+                            gps_Msg.vdop = grab_fields(string, 1) * 100;        // vDOP
+                            break;
+                    }
+                    break;
             }
 
             param++;
@@ -1196,16 +1220,21 @@ static bool gpsNewFrameNMEA(char c)
                     GPS_packetCount++;
                     switch (gps_frame) {
                     case FRAME_GGA:
-                      *gpsPacketLogChar = LOG_NMEA_GGA;
+                        *gpsPacketLogChar = LOG_NMEA_GGA;
-                      frameOK = 1;
+                        frameOK = 1;
-                      if (STATE(GPS_FIX)) {
+                        if (STATE(GPS_FIX)) {
                             gpsSol.llh.lat = gps_Msg.latitude;
                             gpsSol.llh.lon = gps_Msg.longitude;
                             gpsSol.numSat = gps_Msg.numSat;
                             gpsSol.llh.altCm = gps_Msg.altitudeCm;
-                            gpsSol.hdop = gps_Msg.hdop;
                         }
                         break;
+                    case FRAME_GSA:
+                        *gpsPacketLogChar = LOG_NMEA_GSA;
+                        gpsSol.dop.pdop = gps_Msg.pdop;
+                        gpsSol.dop.hdop = gps_Msg.hdop;
+                        gpsSol.dop.vdop = gps_Msg.vdop;
+                        break;
                     case FRAME_RMC:
                         *gpsPacketLogChar = LOG_NMEA_RMC;
                         gpsSol.groundSpeed = gps_Msg.speed;
@@ -1264,6 +1293,17 @@ typedef struct {
     uint32_t uptime;            // milliseconds
 } ubx_nav_status;
 
+typedef struct {
+    uint32_t itow;              // GPS Millisecond Time of Week
+    uint16_t gdop;              // Geometric DOP
+    uint16_t pdop;              // Position DOP
+    uint16_t tdop;              // Time DOP
+    uint16_t vdop;              // Vertical DOP
+    uint16_t hdop;              // Horizontal DOP
+    uint16_t ndop;              // Northing DOP
+    uint16_t edop;              // Easting DOP
+} ubx_nav_dop;
+
 typedef struct {
     uint32_t time;
     int32_t time_nsec;
@@ -1435,6 +1475,7 @@ static bool _new_speed;
 static union {
     ubx_nav_posllh posllh;
     ubx_nav_status status;
+    ubx_nav_dop dop;
     ubx_nav_solution solution;
     ubx_nav_velned velned;
     ubx_nav_pvt pvt;
@@ -1476,13 +1517,18 @@ static bool UBLOX_parse_gps(void)
         if (!next_fix)
             DISABLE_STATE(GPS_FIX);
         break;
+    case MSG_DOP:
+        *gpsPacketLogChar = LOG_UBLOX_DOP;
+        gpsSol.dop.pdop = _buffer.dop.pdop;
+        gpsSol.dop.hdop = _buffer.dop.hdop;
+        gpsSol.dop.vdop = _buffer.dop.vdop;
+        break;
     case MSG_SOL:
         *gpsPacketLogChar = LOG_UBLOX_SOL;
         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
         if (!next_fix)
             DISABLE_STATE(GPS_FIX);
         gpsSol.numSat = _buffer.solution.satellites;
-        gpsSol.hdop = _buffer.solution.position_DOP;
 #ifdef USE_RTC_TIME
         //set clock, when gps time is available
         if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) {
@@ -1508,7 +1554,6 @@ static bool UBLOX_parse_gps(void)
         gpsSetFixState(next_fix);
         _new_position = true;
         gpsSol.numSat = _buffer.pvt.numSV;
-        gpsSol.hdop = _buffer.pvt.pDOP;
         gpsSol.speed3d = (uint16_t) sqrtf(powf(_buffer.pvt.gSpeed / 10, 2.0f) + powf(_buffer.pvt.velD / 10, 2.0f));
         gpsSol.groundSpeed = _buffer.pvt.gSpeed / 10;    // cm/s
         gpsSol.groundCourse = (uint16_t) (_buffer.pvt.headMot / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10

src/main/io/gps.h

@@ -111,12 +111,19 @@ typedef struct gpsLocation_s {
     int32_t altCm;                  // altitude in 0.01m
 } gpsLocation_t;
 
+/* Accuracy of position estimation = device accuracy * DOP */
+typedef struct gpsDilution_s {
+    uint16_t pdop;                  // positional DOP - 3D (* 100)
+    uint16_t hdop;                  // horizontal DOP - 2D (* 100)
+    uint16_t vdop;                  // vertical DOP   - 1D (* 100)
+} gpsDilution_t;
+
 typedef struct gpsSolutionData_s {
     gpsLocation_t llh;
-    uint16_t speed3d;              // speed in 0.1m/s
+    gpsDilution_t dop;
+    uint16_t speed3d;               // speed in 0.1m/s
     uint16_t groundSpeed;           // speed in 0.1m/s
     uint16_t groundCourse;          // degrees * 10
-    uint16_t hdop;                  // generic HDOP value (*100)
     uint8_t numSat;
 } gpsSolutionData_t;
 
@@ -196,7 +203,7 @@ extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];      // Carrier to Noise Rati
 #define GPS_DBHZ_MIN 0
 #define GPS_DBHZ_MAX 55
 
-#define TASK_GPS_RATE       120
+#define TASK_GPS_RATE       100
 #define TASK_GPS_RATE_FAST  1000
 
 void gpsInit(void);

src/main/msp/msp.c

@@ -1500,7 +1500,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
         sbufWriteU16(dst, gpsSol.groundSpeed);
         sbufWriteU16(dst, gpsSol.groundCourse);
         // Added in API version 1.44    
-        sbufWriteU16(dst, gpsSol.hdop);
+        sbufWriteU16(dst, gpsSol.dop.hdop);
         break;
 
     case MSP_COMP_GPS:

src/main/osd/osd_elements.c

@@ -1066,7 +1066,7 @@ static void osdElementGpsSats(osdElementParms_t *element)
         int pos = tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
         if (osdConfig()->gps_sats_show_hdop) { // add on the GPS module HDOP estimate
             element->buff[pos++] = ' ';
-            osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.hdop / 100.0f, "", 1, true, SYM_NONE);
+            osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.dop.hdop / 100.0f, "", 1, true, SYM_NONE);
         }
     }
 }

src/main/telemetry/frsky_hub.c

@@ -288,8 +288,8 @@ static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum)
 {
     uint16_t satellite = gpsSol.numSat;
 
-    if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) { // Every 1s
+    if (gpsSol.dop.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) { // Every 1s
-        satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
+        satellite = constrain(gpsSol.dop.hdop, 0, GPS_MAX_HDOP_VAL);
     }
     int16_t data;
     if (telemetryConfig()->frsky_unit == UNIT_IMPERIAL) {

src/main/telemetry/smartport.c

@@ -796,7 +796,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
 #ifdef USE_GPS
                 if (sensors(SENSOR_GPS)) {
                     // satellite accuracy HDOP: 0 = worst [HDOP > 5.5m], 9 = best [HDOP <= 1.0m]
-                    uint16_t hdop = constrain(scaleRange(gpsSol.hdop, 100, 550, 9, 0), 0, 9) * 100;
+                    uint16_t hdop = constrain(scaleRange(gpsSol.dop.hdop, 100, 550, 9, 0), 0, 9) * 100;
                     smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + hdop + gpsSol.numSat);
                     *clearToSend = false;
                 } else if (featureIsEnabled(FEATURE_GPS)) {

src/main/telemetry/srxl.c

@@ -316,7 +316,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
     groundCourseBcd = dec2bcd(gpsSol.groundCourse);
 
     // HDOP
-    hdop = gpsSol.hdop / 10;
+    hdop = gpsSol.dop.hdop / 10;
     hdop = (hdop > 99) ? 99 : hdop;
     hdopBcd = dec2bcd(hdop);