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];