From 6ed96f9adb02c9aa7ab3e961a57ee9e2bebce58d Mon Sep 17 00:00:00 2001 From: Jan Post Date: Wed, 2 Nov 2022 06:00:34 +0100 Subject: [PATCH] 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 | 1 + src/main/flight/position.c | 6 ++-- src/main/io/gps.c | 63 +++++++++++++++++++++++++++++----- src/main/io/gps.h | 13 +++++-- src/main/msp/msp.c | 2 +- src/main/osd/osd_elements.c | 2 +- src/main/telemetry/frsky_hub.c | 4 +-- src/main/telemetry/smartport.c | 2 +- src/main/telemetry/srxl.c | 2 +- 9 files changed, 74 insertions(+), 21 deletions(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 08bca7a75b..9a4e847fe8 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/flight/position.c b/src/main/flight/position.c index e16cb3b633..8c10c1c8a3 100644 --- a/src/main/flight/position.c +++ b/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) { - gpsTrust = 100.0f / gpsSol.hdop; - // *** TO DO - investigate if we should use vDOP or vACC with UBlox units; this hDOP value is actually pDOP in UBLox code !!! + if (gpsSol.dop.hdop != 0) { + gpsTrust = 100.0f / gpsSol.dop.hdop; + // *** 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); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index edcbdc1b33..677bf090f6 100644 --- a/src/main/io/gps.c +++ b/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; - frameOK = 1; - if (STATE(GPS_FIX)) { + *gpsPacketLogChar = LOG_NMEA_GGA; + frameOK = 1; + 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 diff --git a/src/main/io/gps.h b/src/main/io/gps.h index c4b65e1628..fd12eb51fc 100644 --- a/src/main/io/gps.h +++ b/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); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 383e520d65..85467cd112 100644 --- a/src/main/msp/msp.c +++ b/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: diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 06b276f9ec..374d6049e5 100644 --- a/src/main/osd/osd_elements.c +++ b/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); } } } diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index b028fc48c5..6c91319f3c 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/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 - satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL); + if (gpsSol.dop.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) { // Every 1s + satellite = constrain(gpsSol.dop.hdop, 0, GPS_MAX_HDOP_VAL); } int16_t data; if (telemetryConfig()->frsky_unit == UNIT_IMPERIAL) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 443b2df25e..b6c3e8c69b 100644 --- a/src/main/telemetry/smartport.c +++ b/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)) { diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index d968b51f27..cd244d8157 100644 --- a/src/main/telemetry/srxl.c +++ b/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);