From 09ee27cd973cdffa6ce9edc0146e0c5ad09f8266 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 4 Jul 2022 22:21:39 +1000 Subject: [PATCH] Refactor sat count checks and GPS trust code single minimum GPS satellite setting single required GPS satellite setting CLI Baro vs GPS trust user interface GPS trust refactoring allow arming with GPS_FIX even if not enough sats required sats must be present to arm set required sat count to 8 add blackbox headers --- src/main/blackbox/blackbox.c | 6 +- src/main/cli/settings.c | 6 +- src/main/cms/cms_menu_gps_rescue.c | 10 +- src/main/config/config.c | 10 +- src/main/fc/core.c | 6 +- src/main/fc/parameter_names.h | 5 +- src/main/flight/gps_rescue.c | 19 ++- src/main/flight/gps_rescue.h | 1 - src/main/flight/imu.c | 2 +- src/main/flight/position.c | 149 ++++++++++++-------- src/main/flight/position.h | 6 +- src/main/io/beeper.c | 4 +- src/main/io/gps.c | 28 ++-- src/main/io/gps.h | 4 + src/main/msp/msp.c | 14 +- src/main/msp/msp_protocol.h | 4 +- src/main/osd/osd_elements.c | 4 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/ibus_shared.c | 3 +- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 2 +- src/main/telemetry/srxl.c | 4 +- src/test/unit/arming_prevention_unittest.cc | 1 + src/test/unit/flight_imu_unittest.cc | 1 + src/test/unit/osd_unittest.cc | 2 + src/test/unit/telemetry_hott_unittest.cc | 1 + src/test/unit/telemetry_ibus_unittest.cc | 3 + src/test/unit/vtx_unittest.cc | 1 + 28 files changed, 180 insertions(+), 120 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 6afe5a5c39..9221c3d7fb 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -63,6 +63,7 @@ #include "flight/rpm_filter.h" #include "flight/servos.h" #include "flight/gps_rescue.h" +#include "flight/position.h" #include "io/beeper.h" #include "io/gps.h" @@ -1450,6 +1451,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_NOISE_LPF, "%d", barometerConfig()->baro_noise_lpf); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_VARIO_LPF, "%d", barometerConfig()->baro_vario_lpf); #endif + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALT_SOURCE, "%d", positionConfig()->altSource); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALT_PREFER_BARO, "%d", positionConfig()->altPreferBaro); #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware); #endif @@ -1520,6 +1523,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_REQUIRED_SATS, "%d", gpsConfig()->gpsRequiredSats) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_MINIMUM_SATS, "%d", gpsConfig()->gpsMinimumSats) #ifdef USE_GPS_RESCUE BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ANGLE, "%d", gpsRescueConfig()->angle) @@ -1543,7 +1548,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_DTH, "%d", gpsRescueConfig()->minRescueDth) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ec8ec58fc3..472c1fdaaf 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1011,6 +1011,8 @@ const clivalue_t valueTable[] = { { PARAM_NAME_GPS_UBLOX_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_mode) }, { PARAM_NAME_GPS_SET_HOME_POINT_ONCE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) }, { PARAM_NAME_GPS_USE_3D_SPEED, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_use_3d_speed) }, + { PARAM_NAME_GPS_REQUIRED_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {5, 50}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gpsRequiredSats) }, + { PARAM_NAME_GPS_MINIMUM_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {4, 50}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gpsMinimumSats) }, #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE @@ -1035,7 +1037,6 @@ const clivalue_t valueTable[] = { { PARAM_NAME_GPS_RESCUE_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) }, { PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) }, - { PARAM_NAME_GPS_RESCUE_MIN_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, { PARAM_NAME_GPS_RESCUE_MIN_DTH, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, { PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) }, { PARAM_NAME_GPS_RESCUE_ALT_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) }, @@ -1666,8 +1667,7 @@ const clivalue_t valueTable[] = { // PG_POSITION { "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) }, - { "position_alt_gps_min_sats", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, 50 }, PG_POSITION, offsetof(positionConfig_t, altNumSatsGpsUse) }, - { "position_alt_baro_fallback_sats", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 49 }, PG_POSITION, offsetof(positionConfig_t, altNumSatsBaroFallback) }, + { "position_alt_prefer_baro", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altPreferBaro) }, // PG_MODE_ACTIVATION_CONFIG #if defined(USE_CUSTOM_BOX_NAMES) { "box_user_1_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_1_name) }, diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c index 195a2aa8da..5af24d928d 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/src/main/cms/cms_menu_gps_rescue.c @@ -38,7 +38,7 @@ #include "config/config.h" #include "flight/gps_rescue.h" - +#include "io/gps.h" static uint16_t gpsRescueConfig_angle; //degrees static uint16_t gpsRescueConfig_initialAltitudeM; //meters @@ -47,7 +47,7 @@ static uint16_t gpsRescueConfig_rescueGroundspeed; // centimeters per second static uint16_t gpsRescueConfig_throttleMin; static uint16_t gpsRescueConfig_throttleMax; static uint16_t gpsRescueConfig_throttleHover; -static uint8_t gpsRescueConfig_minSats; +static uint8_t gpsConfig_gpsMinimumSats; static uint16_t gpsRescueConfig_minRescueDth; //meters static uint8_t gpsRescueConfig_allowArmingWithoutFix; static uint16_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD; @@ -133,7 +133,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin ; gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax; gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover; - gpsRescueConfig_minSats = gpsRescueConfig()->minSats; + gpsConfig_gpsMinimumSats = gpsConfig()->gpsMinimumSats; gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; gpsRescueConfig_targetLandingAltitudeM = gpsRescueConfig()->targetLandingAltitudeM; @@ -156,7 +156,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin; gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax; gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover; - gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; + gpsConfigMutable()->gpsMinimumSats = gpsConfig_gpsMinimumSats; gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; gpsRescueConfigMutable()->targetLandingAltitudeM = gpsRescueConfig_targetLandingAltitudeM; @@ -184,7 +184,7 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = { "ASCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 50, 2500, 1 } }, { "DESCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, - { "MIN SATELITES", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } }, + { "MIN SATELITES", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsConfig_gpsMinimumSats, 4, 50, 1 } }, { "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid}, {"BACK", OME_Back, NULL, NULL}, diff --git a/src/main/config/config.c b/src/main/config/config.c index 2ec00c1de6..3eb171e2e7 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -209,11 +209,11 @@ static void validateAndFixRatesSettings(void) } } -static void validateAndFixPositionConfig(void) +static void validateAndFixMinSatsGpsConfig(void) { - if (positionConfig()->altNumSatsBaroFallback >= positionConfig()->altNumSatsGpsUse) { - positionConfigMutable()->altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE; - positionConfigMutable()->altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK; + if (gpsConfig()->gpsMinimumSats >= gpsConfig()->gpsRequiredSats) { + gpsConfigMutable()->gpsRequiredSats = GPS_REQUIRED_SAT_COUNT; + gpsConfigMutable()->gpsMinimumSats = GPS_MINIMUM_SAT_COUNT; } } @@ -603,7 +603,7 @@ static void validateAndFixConfig(void) targetValidateConfiguration(); #endif - validateAndFixPositionConfig(); + validateAndFixMinSatsGpsConfig(); } void validateAndFixGyroConfig(void) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index b59fdbb533..a6d05f5fd0 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -344,7 +344,8 @@ void updateArmingStatus(void) #ifdef USE_GPS_RESCUE if (gpsRescueIsConfigured()) { - if (gpsRescueConfig()->allowArmingWithoutFix || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + if (gpsRescueConfig()->allowArmingWithoutFix || (STATE(GPS_FIX) && (gpsSol.numSat >= gpsConfig()->gpsRequiredSats)) || + ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { unsetArmingDisabled(ARMING_DISABLED_GPS); } else { setArmingDisabled(ARMING_DISABLED_GPS); @@ -560,10 +561,9 @@ void tryArm(void) #ifdef USE_GPS GPS_reset_home_position(); - //beep to indicate arming if (featureIsEnabled(FEATURE_GPS)) { - if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { + if (STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) { beeper(BEEPER_ARMING_GPS_FIX); } else { beeper(BEEPER_ARMING_GPS_NO_FIX); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 77e486c504..9249f4ca16 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -119,6 +119,8 @@ #define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz" #define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz" #define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz" +#define PARAM_NAME_POSITION_ALT_SOURCE "position_alt_source" +#define PARAM_NAME_POSITION_ALT_PREFER_BARO "position_alt_prefer_baro" #ifdef USE_GPS #define PARAM_NAME_GPS_PROVIDER "gps_provider" @@ -130,6 +132,8 @@ #define PARAM_NAME_GPS_UBLOX_MODE "gps_ublox_mode" #define PARAM_NAME_GPS_SET_HOME_POINT_ONCE "gps_set_home_point_once" #define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed" +#define PARAM_NAME_GPS_REQUIRED_SATS "gps_required_sats" +#define PARAM_NAME_GPS_MINIMUM_SATS "gps_minimum_sats" #ifdef USE_GPS_RESCUE #define PARAM_NAME_GPS_RESCUE_ANGLE "gps_rescue_angle" @@ -152,7 +156,6 @@ #define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate" #define PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER "gps_rescue_throttle_hover" #define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks" -#define PARAM_NAME_GPS_RESCUE_MIN_SATS "gps_rescue_min_sats" #define PARAM_NAME_GPS_RESCUE_MIN_DTH "gps_rescue_min_dth" #define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix" #define PARAM_NAME_GPS_RESCUE_ALT_MODE "gps_rescue_alt_mode" diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 0ad228b1f2..ff0f91127e 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -100,7 +100,6 @@ typedef struct { float distanceToHomeM; uint16_t groundSpeedCmS; //cm/s int16_t directionToHome; - uint8_t numSat; float accMagnitude; bool healthy; float errorAngle; @@ -157,7 +156,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .throttleMax = 1600, .throttleHover = 1275, .sanityChecks = RESCUE_SANITY_FS_ONLY, - .minSats = 8, .minRescueDth = 30, .allowArmingWithoutFix = false, .useMag = GPS_RESCUE_USE_MAG, @@ -522,7 +520,7 @@ static void performSanityChecks() } prevAltitudeCm = rescueState.sensor.currentAltitudeCm; - secondsLowSats += rescueState.sensor.numSat < (gpsRescueConfig()->minSats / 2) ? 1 : -1; + secondsLowSats += gpsSol.numSat < (gpsConfig()->gpsMinimumSats) ? 1 : -1; secondsLowSats = constrain(secondsLowSats, 0, 10); if (secondsLowSats == 10) { @@ -558,7 +556,6 @@ static void sensorUpdate() rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f; rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s rescueState.sensor.directionToHome = GPS_directionToHome; - rescueState.sensor.numSat = gpsSol.numSat; rescueState.sensor.errorAngle = (attitude.values.yaw - rescueState.sensor.directionToHome) * 0.1f; // both attitude and direction are in degrees * 10, errorAngle is degrees if (rescueState.sensor.errorAngle <= -180) { @@ -591,12 +588,14 @@ static void sensorUpdate() DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, rescueState.sensor.velocityToHomeCmS); } -// This function checks the following conditions to determine if GPS rescue is available: +// This function flashes "RESCUE N/A" in the OSD if: // 1. sensor healthy - GPS data is being received. -// 2. GPS has a valid fix. -// 3. GPS number of satellites is less than the minimum configured for GPS rescue. -// Note: this function does not take into account the distance from homepoint etc. (gps_rescue_min_dth) and -// is also independent of the gps_rescue_sanity_checks configuration +// 2. GPS has a 3D fix. +// 3. GPS number of satellites is greater than or equal to the minimum configured satellite count. +// Note 1: cannot arm without the required number of sats; +// hence this flashing indicates that after having enough sats, we now have below the minimum and the rescue likely would fail +// Note 2: this function does not take into account the distance from homepoint etc. (gps_rescue_min_dth). +// The sanity checks are independent, this just provides the OSD warning static bool checkGPSRescueIsAvailable(void) { static timeUs_t previousTimeUs = 0; // Last time LowSat was checked @@ -628,7 +627,7 @@ static bool checkGPSRescueIsAvailable(void) noGPSfix = false; } - secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < gpsRescueConfig()->minSats) ? 1 : -1), 0, 2); + secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < gpsConfig()->gpsMinimumSats) ? 1 : -1), 0, 2); if (secondsLowSats == 2) { lowsats = true; result = false; diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 615d7066d7..5d514bf0fd 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -32,7 +32,6 @@ typedef struct gpsRescue_s { uint16_t throttleMax; uint16_t throttleHover; uint8_t velP, velI, velD; - uint8_t minSats; uint16_t minRescueDth; //meters uint8_t sanityChecks; uint8_t allowArmingWithoutFix; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c4fb38ae4f..4ac647ae7f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -488,7 +488,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) } #endif #if defined(USE_GPS) - if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) { + if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > gpsConfig()->gpsMinimumSats && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) { // Use GPS course over ground to correct attitude.values.yaw courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); useCOG = true; diff --git a/src/main/flight/position.c b/src/main/flight/position.c index dc91dc2972..3cbc7cb47e 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -58,12 +58,11 @@ typedef enum { GPS_ONLY } altSource_e; -PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 3); PG_RESET_TEMPLATE(positionConfig_t, positionConfig, .altSource = DEFAULT, - .altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE, - .altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK, + .altPreferBaro = 100, ); #ifdef USE_VARIO @@ -82,23 +81,20 @@ static bool altitudeOffsetSetGPS = false; void calculateEstimatedAltitude() { - static float baroAltOffset = 0; - static float gpsAltOffset = 0; - - float baroAlt = 0; + float baroAltCm = 0; + float gpsAltCm = 0; float baroAltVelocity = 0; - float gpsAlt = 0; - uint8_t gpsNumSat = 0; - -#if defined(USE_GPS) && defined(USE_VARIO) - float gpsVertSpeed = 0; -#endif float gpsTrust = 0.3; //conservative default bool haveBaroAlt = false; bool haveGpsAlt = false; +#if defined(USE_GPS) && defined(USE_VARIO) + float gpsVertSpeed = 0; +#endif + +// *** Set baroAlt once its calibration is complete (cannot arm until it is) #ifdef USE_BARO if (sensors(SENSOR_BARO)) { - static float lastBaroAlt = 0; + static float lastBaroAltCm = 0; static bool initBaroFilter = false; if (!initBaroFilter) { const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f; @@ -107,96 +103,133 @@ void calculateEstimatedAltitude() pt2FilterInit(&baroDerivativeLpf, gain); initBaroFilter = true; } - baroAlt = baroUpsampleAltitude(); - const float baroAltVelocityRaw = (baroAlt - lastBaroAlt) * TASK_ALTITUDE_RATE_HZ; // cm/s + baroAltCm = baroUpsampleAltitude(); + const float baroAltVelocityRaw = (baroAltCm - lastBaroAltCm) * TASK_ALTITUDE_RATE_HZ; // cm/s baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw); - lastBaroAlt = baroAlt; + lastBaroAltCm = baroAltCm; if (baroIsCalibrated()) { haveBaroAlt = true; } } #endif + // *** Check GPS for 3D fix, set haveGpsAlt if we have a 3D fix, and GPS Trust based on hdop, or leave at default of 0.3 #ifdef USE_GPS if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - gpsAlt = gpsSol.llh.altCm; - gpsNumSat = gpsSol.numSat; + // Need a 3D fix, which requires min 4 sats + // if not, gpsAltCm remains zero, haveGpsAlt stays false, and gpsTrust is zero. + gpsAltCm = gpsSol.llh.altCm; #ifdef USE_VARIO gpsVertSpeed = GPS_verticalSpeedInCmS; #endif - haveGpsAlt = true; + haveGpsAlt = true; // remains false if no 3D fix +#ifdef USE_GPS if (gpsSol.hdop != 0) { gpsTrust = 100.0 / gpsSol.hdop; + // *** TO DO - investigate if we should use vDOP or vACC with UBlox units; this hDOP value is actually pDOP in UBLox code !!! } - // always use at least 10% of other sources besides gps if available +#endif + // always use at least 10% of other sources besides gps if available; limit effect of HDOP gpsTrust = MIN(gpsTrust, 0.9f); + // With a 3D fix, GPS trust starts at 0.3 + } else { + gpsTrust = 0.0f; // don't trust GPS if no sensor or 3D fix } #endif - if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) { - baroAltOffset = baroAlt; - altitudeOffsetSetBaro = true; - } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) { - altitudeOffsetSetBaro = false; - } - - baroAlt -= baroAltOffset; - - int goodGpsSats = 0; - int badGpsSats = -1; - - if (haveBaroAlt) { - goodGpsSats = positionConfig()->altNumSatsGpsUse; - badGpsSats = positionConfig()->altNumSatsBaroFallback; - } - + // *** Zero Baro Altitude on arming (every time we re-arm, also) + // note that arming is blocked until baro 'calibration' (baro ground zeroing) is complete, no need to check status of haveBaroAlt + // this code adds a secondary zeroing to whatever baro altitude value exists on arming + // since props spin on arming, we want the last value before arming + // provided that this function is called before significant motor spin-up has occured, this may not be a big problem +#ifdef USE_BARO + static float baroAltOffsetCm = 0; if (ARMING_FLAG(ARMED)) { - if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) { - gpsAltOffset = gpsAlt - baroAlt; + baroAltCm -= baroAltOffsetCm; // use the last offset value from the disarm period + altitudeOffsetSetBaro = true; // inevitable, but needed if no GPS + } else { + baroAltOffsetCm = baroAltCm; // while disarmed, keep capturing current altitude to zero any offset once armed + } +#endif + + // *** Zero GPS Altitude on every arm or re-arm using most recent disarmed values + // but do not use GPS if there are not the required satellites + + // note that if GPS Rescue is enabled, a Home fix is required and reset when the user arms, so the checks below aren't needed + // tryArm() runs code that sets GPS_FIX_HOME, and if the result is no home fix, arming is blocked + // a check like this is essential for GPS Rescue + // if an altitude hold function was enabled, the same test would be good for ensuring a reasonable altitude estimate before takeoff + + // If GPS Rescue is not enabled, the user can arm without a Home fix, or indeed without a 3D fix, with GPS active + // While disarmed, we use the current GPS value, regardless of GPS quality + // Also while disarmed, we monitor for a 3D fix and at least the required number of satellites + // If these are achieved before arming, we lock the offset at the gpsAltitude value + // If we don't, we wait until we get a 3D fix, and then correct the offset using the baro value + // This won't be very accurate, but all we need is a 3D fix. + // Note that without the 3D fix, GPS trust will be zero, and on getting a 3D fix, will depend on hDOP +#ifdef USE_GPS + static float gpsAltOffsetCm = 0; + if (ARMING_FLAG(ARMED)) { + gpsAltCm -= gpsAltOffsetCm; // while armed, use the last offset value from the disarm period + if (!altitudeOffsetSetGPS && haveBaroAlt && haveGpsAlt) { + // if we get a 3D fix after not getting a decent altitude offset, zero GPS to the Baro reading + gpsAltOffsetCm = gpsAltCm - baroAltCm; // set GPS to Baro altitude once we get a GPS fix altitudeOffsetSetGPS = true; - } else if (gpsNumSat <= badGpsSats) { - altitudeOffsetSetGPS = false; } - } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) { - altitudeOffsetSetGPS = false; + } else { + gpsAltOffsetCm = gpsAltCm; // while disarmed, keep capturing current altitude, to zero any offset once armed + altitudeOffsetSetGPS = (haveGpsAlt && gpsConfig()->gpsRequiredSats); + // if no 3D fix and not enough sats, set the GPS offset flag to false, and use baro to correct once we get a fix } +#endif - gpsAlt -= gpsAltOffset; - - if (!altitudeOffsetSetGPS) { - haveGpsAlt = false; - gpsTrust = 0.0f; + // *** adjust gpsTrust, favouring Baro increasingly when there is a discrepancy of more than a meter + // favour GPS if Baro reads negative, this happens due to ground effects + float gpsTrustModifier = gpsTrust; + const float absDifferenceM = fabsf(gpsAltCm - baroAltCm) * positionConfig()->altPreferBaro / 10000.0f; + if (absDifferenceM > 1.0f && baroAltCm > -100.0f) { // significant difference, and baro altitude not negative + gpsTrustModifier /= absDifferenceM; } + // eg if discrepancy is 3m and GPS trust was 0.9, it would now be 0.3 + // *** If we have a GPS with 3D fix and a Baro signal, blend them if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) { if (ARMING_FLAG(ARMED)) { - estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust); + estimatedAltitudeCm = gpsAltCm * gpsTrustModifier + baroAltCm * (1 - gpsTrustModifier); } else { - estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro + estimatedAltitudeCm = gpsAltCm; + //absolute GPS altitude is shown before arming, ignoring baro (I have no clue why this is) } + #ifdef USE_VARIO // baro is a better source for vario, so ignore gpsVertSpeed estimatedVario = calculateEstimatedVario(baroAltVelocity); #endif + + // *** If we have a GPS but no baro, and are in Default or GPS_ONLY modes, use GPS values } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) { - estimatedAltitudeCm = gpsAlt; + estimatedAltitudeCm = gpsAltCm; #if defined(USE_VARIO) && defined(USE_GPS) estimatedVario = gpsVertSpeed; #endif + + // *** If we have a Baro, and can work with it in Default or Baro Only modes } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) { - estimatedAltitudeCm = baroAlt; + estimatedAltitudeCm = baroAltCm; + #ifdef USE_VARIO estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s #endif } + // Note that if we have no GPS but user chooses GPS Only, or no Baro but user chooses Baro only, then the reported altitude will be zero + // The latter may cause GPS rescue to fail, but the user should notice an absence of altitude values. + DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust)); - DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt); - DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt); -#ifdef USE_VARIO - DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario); -#endif + DEBUG_SET(DEBUG_ALTITUDE, 1, baroAltCm); + DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm); + DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedAltitudeCm); } bool isAltitudeOffset(void) diff --git a/src/main/flight/position.h b/src/main/flight/position.h index bc32d591f1..e53f7dc939 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -23,13 +23,9 @@ #include "common/time.h" #define TASK_ALTITUDE_RATE_HZ 120 -#define POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE 10 -#define POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK 7 - typedef struct positionConfig_s { uint8_t altSource; - uint8_t altNumSatsGpsUse; - uint8_t altNumSatsBaroFallback; + uint8_t altPreferBaro; } positionConfig_t; PG_DECLARE(positionConfig_t, positionConfig); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 8413f643d3..a68f1cf006 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -358,8 +358,8 @@ void beeperWarningBeeps(uint8_t beepCount) static void beeperGpsStatus(void) { if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) { - // if GPS fix then beep out number of satellites - if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { + // if GPS 3D fix and at least the minimum number available, then beep out number of satellites + if (STATE(GPS_FIX) && gpsSol.numSat > gpsConfig()->gpsMinimumSats) { uint8_t i = 0; do { beep_multiBeeps[i++] = 5; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 6742c3ed92..e0e073fb30 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -273,7 +273,7 @@ typedef enum { gpsData_t gpsData; -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 1); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = GPS_UBLOX, @@ -284,7 +284,9 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .gps_ublox_mode = UBLOX_AIRBORNE, .gps_set_home_point_once = false, .gps_use_3d_speed = false, - .sbas_integrity = false + .sbas_integrity = false, + .gpsRequiredSats = GPS_REQUIRED_SAT_COUNT, + .gpsMinimumSats = GPS_MINIMUM_SAT_COUNT ); static void shiftPacketLog(void) @@ -757,7 +759,7 @@ void gpsInitHardware(void) static void updateGpsIndicator(timeUs_t currentTimeUs) { static uint32_t GPSLEDTime; - if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) { + if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsConfig()->gpsRequiredSats)) { GPSLEDTime = currentTimeUs + 150000; LED1_TOGGLE; } @@ -867,18 +869,15 @@ void gpsUpdate(timeUs_t currentTimeUs) DISABLE_STATE(GPS_FIX_HOME); } - uint8_t minSats = 5; - #if defined(USE_GPS_RESCUE) if (gpsRescueIsConfigured()) { updateGPSRescueState(); - minSats = gpsRescueConfig()->minSats; } #endif static bool hasFix = false; - if (STATE(GPS_FIX)) { - if (gpsIsHealthy() && gpsSol.numSat >= minSats && !hasFix) { + if (STATE(GPS_FIX_HOME)) { + if (gpsIsHealthy() && gpsSol.numSat >= gpsConfig()->gpsRequiredSats && !hasFix) { // ready beep sequence on fix or requirements for gps rescue met. beeper(BEEPER_READY_BEEP); hasFix = true; @@ -1804,11 +1803,12 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize) void GPS_reset_home_position(void) { if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) { - if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { + if (STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) { + // requires the full sat count to say we have a home fix GPS_home[GPS_LATITUDE] = gpsSol.llh.lat; GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon; - GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc - // Set ground altitude + GPS_calc_longitude_scaling(gpsSol.llh.lat); + // need an initial value for distance and bearing calcs, and to set ground altitude ENABLE_STATE(GPS_FIX_HOME); } } @@ -1833,7 +1833,7 @@ void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t void GPS_calculateDistanceAndDirectionToHome(void) { - if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything + if (STATE(GPS_FIX_HOME)) { uint32_t dist; int32_t dir; GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir); @@ -1841,6 +1841,7 @@ void GPS_calculateDistanceAndDirectionToHome(void) GPS_distanceToHomeCm = dist; // cm/sec GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees } else { + // If we don't have home set, do not display anything GPS_distanceToHome = 0; GPS_distanceToHomeCm = 0; GPS_directionToHome = 0; @@ -1849,7 +1850,8 @@ void GPS_calculateDistanceAndDirectionToHome(void) void onGpsNewData(void) { - if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) { + if (!(STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsMinimumSats)) { + // if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue return; } diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 26fb4685f7..5da3236aa6 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -82,6 +82,8 @@ typedef enum { UBLOX_ACK_GOT_NACK } ubloxAckState_e; +#define GPS_REQUIRED_SAT_COUNT 8 +#define GPS_MINIMUM_SAT_COUNT 4 #define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600 typedef struct gpsConfig_s { @@ -94,6 +96,8 @@ typedef struct gpsConfig_s { uint8_t gps_set_home_point_once; uint8_t gps_use_3d_speed; uint8_t sbas_integrity; + uint8_t gpsRequiredSats; + uint8_t gpsMinimumSats; } gpsConfig_t; PG_DECLARE(gpsConfig_t, gpsConfig); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 0f1a3e1bfb..91818d5d7c 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1452,6 +1452,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) // Added in API version 1.43 sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once); sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo); + // Added in API version 1.45 + sbufWriteU8(dst, gpsConfig()->gpsRequiredSats); + sbufWriteU8(dst, gpsConfig()->gpsMinimumSats); break; case MSP_RAW_GPS: @@ -1492,7 +1495,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, gpsRescueConfig()->throttleMax); sbufWriteU16(dst, gpsRescueConfig()->throttleHover); sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); - sbufWriteU8(dst, gpsRescueConfig()->minSats); + sbufWriteU8(dst, 0); // not required in API 1.44, gpsRescueConfig()->minSats + // Added in API version 1.43 sbufWriteU16(dst, gpsRescueConfig()->ascendRate); sbufWriteU16(dst, gpsRescueConfig()->descendRate); @@ -2690,6 +2694,12 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src); gpsConfigMutable()->gps_ublox_use_galileo = sbufReadU8(src); } + if (sbufBytesRemaining(src) >= 2) { + // Added in API version 1.45 + gpsConfigMutable()->gpsRequiredSats = sbufReadU8(src); + gpsConfigMutable()->gpsMinimumSats = sbufReadU8(src); + } + break; #ifdef USE_GPS_RESCUE @@ -2702,7 +2712,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, gpsRescueConfigMutable()->throttleMax = sbufReadU16(src); gpsRescueConfigMutable()->throttleHover = sbufReadU16(src); gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); - gpsRescueConfigMutable()->minSats = sbufReadU8(src); + sbufReadU8(src); // not used since 1.43, was gps rescue minSats if (sbufBytesRemaining(src) >= 6) { // Added in API version 1.43 gpsRescueConfigMutable()->ascendRate = sbufReadU16(src); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index ec2d56fe63..8ae37bce39 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -315,9 +315,9 @@ #define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag #define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings #define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc) -#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration +#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration including from 1.44 minSats //DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration -#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed and sanityChecks #define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P #define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time) #define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time) diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 313e173cd9..6da7c44d91 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -1905,9 +1905,9 @@ void osdUpdateAlarms(void) } #ifdef USE_GPS - if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5) + if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < gpsConfig()->gpsMinimumSats) #ifdef USE_GPS_RESCUE - || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured()) + || ((gpsSol.numSat < gpsConfig()->gpsRequiredSats) && gpsRescueIsConfigured()) #endif ) { SET_BLINK(OSD_GPS_SATS); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index cf3181d435..0b33b89d07 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -218,7 +218,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) return; } - if (gpsSol.numSat >= 5) { + if (gpsSol.numSat >= gpsConfig()->gpsMinimumSats) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D; } else { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 69eac4d746..1171f6ebfd 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -306,8 +306,9 @@ static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value) uint16_t gpsFixType = 0; uint16_t sats = 0; + uint8_t minSats = gpsConfig()->gpsMinimumSats; if (sensors(SENSOR_GPS)) { - gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < 5 ? 2 : 3); + gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < minSats ? 2 : 3); sats = gpsSol.numSat; if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) { result = true; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 894a6092a5..3a0d8dcbf2 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -137,7 +137,7 @@ static void ltm_gframe(void) if (!STATE(GPS_FIX)) gps_fix_type = 1; - else if (gpsSol.numSat < 5) + else if (gpsSol.numSat < gpsConfig()->gpsMinimumSats) gps_fix_type = 2; else gps_fix_type = 3; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ee47fbe999..1c522da250 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -309,7 +309,7 @@ void mavlinkSendPosition(void) gpsFixType = 1; } else { - if (gpsSol.numSat < 5) { + if (gpsSol.numSat < gpsConfig()->gpsMinimumSats) { gpsFixType = 2; } else { diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 3c49853475..ec443aa25a 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -302,7 +302,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint8_t hdopBcd, gpsFlags; - if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < gpsConfig()->gpsMinimumSats) { return false; } @@ -370,7 +370,7 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) uint8_t numSatBcd, altitudeHighBcd; bool timeProvided = false; - if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < gpsConfig()->gpsMinimumSats) { return false; } diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index af2029986f..e798400a5f 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -58,6 +58,7 @@ extern "C" { PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); + PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); float rcCommand[4]; float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 534d883df6..2d7b1fb17f 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -64,6 +64,7 @@ extern "C" { PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); + PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = 0 diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index d1d7f9c9c8..2f28bd6cfe 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1190,6 +1190,8 @@ TEST_F(OsdTest, TestGpsElements) { // given osdElementConfigMutable()->item_pos[OSD_GPS_SATS] = OSD_POS(2, 4) | OSD_PROFILE_1_FLAG; + gpsConfigMutable()->gpsMinimumSats = GPS_MINIMUM_SAT_COUNT; + gpsConfigMutable()->gpsRequiredSats = GPS_REQUIRED_SAT_COUNT; sensorsSet(SENSOR_GPS); osdAnalyzeActiveElements(); diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 25325ca5d2..f5d49d4229 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -51,6 +51,7 @@ extern "C" { #include "telemetry/hott.h" PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); uint16_t testBatteryVoltage = 0; int32_t testAmperage = 0; diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index c291cafed3..5c4aa2f248 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -22,6 +22,7 @@ extern "C" { #include "platform.h" #include "common/utils.h" #include "pg/pg.h" +#include "pg/pg_ids.h" #include "drivers/serial.h" #include "io/serial.h" #include "io/gps.h" @@ -36,6 +37,8 @@ extern "C" { #include "sensors/acceleration.h" #include "scheduler/scheduler.h" #include "fc/tasks.h" + +PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); } #include "unittest_macros.h" diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index f9f3311f89..7e222d5947 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -61,6 +61,7 @@ extern "C" { PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); + PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); float rcCommand[4]; float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];