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 | 145 +++++++++++++++++----------- 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, 178 insertions(+), 118 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 6afe5a5c3..9221c3d7f 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 ec8ec58fc..472c1fdaa 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 195a2aa8d..5af24d928 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 2ec00c1de..3eb171e2e 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 b59fdbb53..a6d05f5fd 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 77e486c50..9249f4ca1 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 0ad228b1f..ff0f91127 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 615d7066d..5d514bf0f 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 c4fb38ae4..4ac647ae7 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 dc91dc297..3cbc7cb47 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)) { + 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)) { - if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) { - gpsAltOffset = gpsAlt - baroAlt; + 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 bc32d591f..e53f7dc93 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 8413f643d..a68f1cf00 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 6742c3ed9..e0e073fb3 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 26fb4685f..5da3236aa 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 0f1a3e1bf..91818d5d7 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 ec2d56fe6..8ae37bce3 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 313e173cd..6da7c44d9 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 cf3181d43..0b33b89d0 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 69eac4d74..1171f6ebf 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 894a6092a..3a0d8dcbf 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 ee47fbe99..1c522da25 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 3c4985347..ec443aa25 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 af2029986..e798400a5 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 534d883df..2d7b1fb17 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 d1d7f9c9c..2f28bd6cf 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 25325ca5d..f5d49d422 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 c291cafed..5c4aa2f24 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 f9f3311f8..7e222d594 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]; -- 2.11.4.GIT