Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Parse GPS DOP values (#11912)

Browse source 
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
This commit is contained in:
Jan Post 2022-11-02 06:00:34 +01:00 • committed by GitHub
parent 4b6da37da1
commit 6ed96f9adb
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 74 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

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

6
src/main/flight/position.c
View file

@ -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);

63
src/main/io/gps.c
View file

@ -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

13
src/main/io/gps.h
View file

@ -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);

2
src/main/msp/msp.c
View file

@ -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:

2
src/main/osd/osd_elements.c
View file

@ -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);
}
}
}

4
src/main/telemetry/frsky_hub.c
View file

@ -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) {

2
src/main/telemetry/smartport.c
View file

@ -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)) {

2
src/main/telemetry/srxl.c
View file

@ -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);