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

remove gps_required and gps_minimum sats and revert to gps_rescue_min_sats

This commit is contained in:
ctzsnooze 2022-10-19 15:58:54 +11:00
parent c5468981e6
commit 341d65becf
22 changed files with 41 additions and 63 deletions

View file

@ -1525,8 +1525,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider) 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_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_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 #ifdef USE_GPS_RESCUE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth)
@ -1548,6 +1546,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover) 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_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_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)

View file

@ -1009,8 +1009,6 @@ 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_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_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_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 #ifdef USE_GPS_RESCUE
// PG_GPS_RESCUE // PG_GPS_RESCUE
@ -1033,6 +1031,7 @@ const clivalue_t valueTable[] = {
{ 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_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_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_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_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_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },

View file

@ -38,7 +38,6 @@
#include "config/config.h" #include "config/config.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "io/gps.h"
static uint16_t gpsRescueConfig_minRescueDth; //meters static uint16_t gpsRescueConfig_minRescueDth; //meters
static uint8_t gpsRescueConfig_altitudeMode; static uint8_t gpsRescueConfig_altitudeMode;
@ -57,7 +56,7 @@ static uint16_t gpsRescueConfig_throttleMin;
static uint16_t gpsRescueConfig_throttleMax; static uint16_t gpsRescueConfig_throttleMax;
static uint16_t gpsRescueConfig_throttleHover; static uint16_t gpsRescueConfig_throttleHover;
static uint8_t gpsConfig_gpsRequiredSats; static uint8_t gpsRescueConfig_minSats;
static uint8_t gpsRescueConfig_allowArmingWithoutFix; static uint8_t gpsRescueConfig_allowArmingWithoutFix;
static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD; static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
@ -150,7 +149,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax; gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover; gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover;
gpsConfig_gpsRequiredSats = gpsConfig()->gpsRequiredSats; gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
return NULL; return NULL;
@ -178,7 +177,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax; gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover; gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover;
gpsConfigMutable()->gpsRequiredSats = gpsConfig_gpsRequiredSats; gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
return NULL; return NULL;
@ -205,7 +204,7 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } }, { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } },
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } }, { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } },
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsConfig_gpsRequiredSats, 4, 50, 1 } }, { "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } },
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },
{ "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid}, { "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},

View file

@ -209,14 +209,6 @@ static void validateAndFixRatesSettings(void)
} }
} }
static void validateAndFixMinSatsGpsConfig(void)
{
if (gpsConfig()->gpsMinimumSats >= gpsConfig()->gpsRequiredSats) {
gpsConfigMutable()->gpsRequiredSats = GPS_REQUIRED_SAT_COUNT;
gpsConfigMutable()->gpsMinimumSats = GPS_MINIMUM_SAT_COUNT;
}
}
static void validateAndFixConfig(void) static void validateAndFixConfig(void)
{ {
#if !defined(USE_QUAD_MIXER_ONLY) #if !defined(USE_QUAD_MIXER_ONLY)
@ -602,8 +594,6 @@ static void validateAndFixConfig(void)
// This should be done at the end of the validation // This should be done at the end of the validation
targetValidateConfiguration(); targetValidateConfiguration();
#endif #endif
validateAndFixMinSatsGpsConfig();
} }
void validateAndFixGyroConfig(void) void validateAndFixGyroConfig(void)

View file

@ -345,7 +345,7 @@ void updateArmingStatus(void)
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
if (gpsRescueIsConfigured()) { if (gpsRescueIsConfigured()) {
if (gpsRescueConfig()->allowArmingWithoutFix || (STATE(GPS_FIX) && (gpsSol.numSat >= gpsConfig()->gpsRequiredSats)) || if (gpsRescueConfig()->allowArmingWithoutFix || (STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) ||
ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
unsetArmingDisabled(ARMING_DISABLED_GPS); unsetArmingDisabled(ARMING_DISABLED_GPS);
} else { } else {
@ -577,7 +577,7 @@ void tryArm(void)
GPS_reset_home_position(); GPS_reset_home_position();
//beep to indicate arming //beep to indicate arming
if (featureIsEnabled(FEATURE_GPS)) { if (featureIsEnabled(FEATURE_GPS)) {
if (STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) { if (STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
beeper(BEEPER_ARMING_GPS_FIX); beeper(BEEPER_ARMING_GPS_FIX);
} else { } else {
beeper(BEEPER_ARMING_GPS_NO_FIX); beeper(BEEPER_ARMING_GPS_NO_FIX);

View file

@ -133,8 +133,6 @@
#define PARAM_NAME_GPS_UBLOX_MODE "gps_ublox_mode" #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_SET_HOME_POINT_ONCE "gps_set_home_point_once"
#define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed" #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 #ifdef USE_GPS_RESCUE
#define PARAM_NAME_GPS_RESCUE_MIN_START_DIST "gps_rescue_min_start_dist" #define PARAM_NAME_GPS_RESCUE_MIN_START_DIST "gps_rescue_min_start_dist"
@ -156,6 +154,7 @@
#define PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER "gps_rescue_throttle_hover" #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_SANITY_CHECKS "gps_rescue_sanity_checks"
#define PARAM_NAME_GPS_RESCUE_MIN_SATS "gps_rescue_min_sats"
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix" #define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p" #define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p"

View file

@ -167,6 +167,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.allowArmingWithoutFix = false, .allowArmingWithoutFix = false,
.sanityChecks = RESCUE_SANITY_FS_ONLY, .sanityChecks = RESCUE_SANITY_FS_ONLY,
.minSats = 8,
.throttleP = 15, .throttleP = 15,
.throttleI = 15, .throttleI = 15,
@ -507,7 +508,7 @@ static void performSanityChecks(void)
} }
} }
secondsLowSats += (!STATE(GPS_FIX) || (gpsSol.numSat < gpsConfig()->gpsMinimumSats)) ? 1 : -1; secondsLowSats += (!STATE(GPS_FIX) || (gpsSol.numSat < GPS_MIN_SAT_COUNT)) ? 1 : -1;
secondsLowSats = constrain(secondsLowSats, 0, 10); secondsLowSats = constrain(secondsLowSats, 0, 10);
if (secondsLowSats == 10) { if (secondsLowSats == 10) {
@ -654,7 +655,7 @@ static bool checkGPSRescueIsAvailable(void)
noGPSfix = false; noGPSfix = false;
} }
secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < gpsConfig()->gpsMinimumSats) ? 1 : -1), 0, 2); secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < GPS_MIN_SAT_COUNT) ? 1 : -1), 0, 2);
if (secondsLowSats == 2) { if (secondsLowSats == 2) {
lowsats = true; lowsats = true;
result = false; result = false;

View file

@ -31,6 +31,7 @@ typedef struct gpsRescue_s {
uint16_t throttleMin; uint16_t throttleMin;
uint16_t throttleMax; uint16_t throttleMax;
uint16_t throttleHover; uint16_t throttleHover;
uint8_t minSats;
uint8_t velP, velI, velD; uint8_t velP, velI, velD;
uint16_t minRescueDth; //meters uint16_t minRescueDth; //meters
uint8_t sanityChecks; uint8_t sanityChecks;

View file

@ -490,7 +490,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
} }
#endif #endif
#if defined(USE_GPS) #if defined(USE_GPS)
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > gpsConfig()->gpsMinimumSats && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) { if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
// Use GPS course over ground to correct attitude.values.yaw // Use GPS course over ground to correct attitude.values.yaw
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true; useCOG = true;

View file

@ -359,7 +359,7 @@ static void beeperGpsStatus(void)
{ {
if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) { if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) {
// if GPS 3D fix and at least the minimum number available, then beep out number of satellites // 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) { if (STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT) {
uint8_t i = 0; uint8_t i = 0;
do { do {
beep_multiBeeps[i++] = 5; beep_multiBeeps[i++] = 5;

View file

@ -273,7 +273,7 @@ typedef enum {
gpsData_t gpsData; gpsData_t gpsData;
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.provider = GPS_UBLOX, .provider = GPS_UBLOX,
@ -285,8 +285,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.gps_set_home_point_once = false, .gps_set_home_point_once = false,
.gps_use_3d_speed = 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) static void shiftPacketLog(void)
@ -764,7 +762,7 @@ void gpsInitHardware(void)
static void updateGpsIndicator(timeUs_t currentTimeUs) static void updateGpsIndicator(timeUs_t currentTimeUs)
{ {
static uint32_t GPSLEDTime; static uint32_t GPSLEDTime;
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsConfig()->gpsRequiredSats)) { if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) {
GPSLEDTime = currentTimeUs + 150000; GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE; LED1_TOGGLE;
} }
@ -886,7 +884,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
// while disarmed, beep when requirements for a home fix are met // while disarmed, beep when requirements for a home fix are met
// ?? should we also beep if home fix requirements first appear after arming? // ?? should we also beep if home fix requirements first appear after arming?
if (!hasBeeped && STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) { if (!hasBeeped && STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
beeper(BEEPER_READY_BEEP); beeper(BEEPER_READY_BEEP);
hasBeeped = true; hasBeeped = true;
} }
@ -1810,7 +1808,7 @@ void GPS_reset_home_position(void)
// runs, if GPS is defined, on arming via tryArm() in core.c, and on gyro cal via processRcStickPositions() in rc_controls.c // runs, if GPS is defined, on arming via tryArm() in core.c, and on gyro cal via processRcStickPositions() in rc_controls.c
{ {
if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) { if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
if (STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) { if (STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
// those checks are always true for tryArm, but may not be true for gyro cal // those checks are always true for tryArm, but may not be true for gyro cal
GPS_home[GPS_LATITUDE] = gpsSol.llh.lat; GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon; GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
@ -1860,7 +1858,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void) void onGpsNewData(void)
{ {
if (!(STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsMinimumSats)) { if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
// if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue // if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue
return; return;
} }

View file

@ -28,6 +28,7 @@
#define GPS_DEGREES_DIVIDER 10000000L #define GPS_DEGREES_DIVIDER 10000000L
#define GPS_X 1 #define GPS_X 1
#define GPS_Y 0 #define GPS_Y 0
#define GPS_MIN_SAT_COUNT 4 // number of sats to trigger low sat count sanity check
typedef enum { typedef enum {
GPS_LATITUDE, GPS_LATITUDE,
@ -82,8 +83,6 @@ typedef enum {
UBLOX_ACK_GOT_NACK UBLOX_ACK_GOT_NACK
} ubloxAckState_e; } ubloxAckState_e;
#define GPS_REQUIRED_SAT_COUNT 8
#define GPS_MINIMUM_SAT_COUNT 4
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600 #define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
typedef struct gpsConfig_s { typedef struct gpsConfig_s {
@ -96,8 +95,6 @@ typedef struct gpsConfig_s {
uint8_t gps_set_home_point_once; uint8_t gps_set_home_point_once;
uint8_t gps_use_3d_speed; uint8_t gps_use_3d_speed;
uint8_t sbas_integrity; uint8_t sbas_integrity;
uint8_t gpsRequiredSats;
uint8_t gpsMinimumSats;
} gpsConfig_t; } gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig); PG_DECLARE(gpsConfig_t, gpsConfig);

View file

@ -1489,9 +1489,6 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
// Added in API version 1.43 // Added in API version 1.43
sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once); sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once);
sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo); sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo);
// Added in API version 1.45
sbufWriteU8(dst, gpsConfig()->gpsRequiredSats);
sbufWriteU8(dst, gpsConfig()->gpsMinimumSats);
break; break;
case MSP_RAW_GPS: case MSP_RAW_GPS:
@ -1532,7 +1529,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
sbufWriteU16(dst, gpsRescueConfig()->throttleMax); sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
sbufWriteU16(dst, gpsRescueConfig()->throttleHover); sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
sbufWriteU8(dst, 0); // not required in API 1.44, gpsRescueConfig()->minSats sbufWriteU8(dst, gpsRescueConfig()->minSats);
// Added in API version 1.43 // Added in API version 1.43
sbufWriteU16(dst, gpsRescueConfig()->ascendRate); sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
@ -2740,12 +2737,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src); gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src);
gpsConfigMutable()->gps_ublox_use_galileo = 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; break;
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
@ -2758,7 +2749,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src); gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
gpsRescueConfigMutable()->throttleHover = sbufReadU16(src); gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
sbufReadU8(src); // not used since 1.43, was gps rescue minSats gpsRescueConfigMutable()->minSats = sbufReadU8(src);
if (sbufBytesRemaining(src) >= 6) { if (sbufBytesRemaining(src) >= 6) {
// Added in API version 1.43 // Added in API version 1.43
gpsRescueConfigMutable()->ascendRate = sbufReadU16(src); gpsRescueConfigMutable()->ascendRate = sbufReadU16(src);

View file

@ -315,7 +315,7 @@
#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag #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_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_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration including from 1.44 minSats #define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration //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 and sanityChecks #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_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P

View file

@ -1953,9 +1953,9 @@ void osdUpdateAlarms(void)
} }
#ifdef USE_GPS #ifdef USE_GPS
if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < gpsConfig()->gpsMinimumSats) if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < GPS_MIN_SAT_COUNT)
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
|| ((gpsSol.numSat < gpsConfig()->gpsRequiredSats) && gpsRescueIsConfigured()) || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
#endif #endif
) { ) {
SET_BLINK(OSD_GPS_SATS); SET_BLINK(OSD_GPS_SATS);

View file

@ -218,7 +218,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
return; return;
} }
if (gpsSol.numSat >= gpsConfig()->gpsMinimumSats) { if (gpsSol.numSat >= GPS_MIN_SAT_COUNT) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else { } else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;

View file

@ -307,9 +307,8 @@ static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value)
uint16_t gpsFixType = 0; uint16_t gpsFixType = 0;
uint16_t sats = 0; uint16_t sats = 0;
uint8_t minSats = gpsConfig()->gpsMinimumSats;
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < minSats ? 2 : 3); gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < GPS_MIN_SAT_COUNT ? 2 : 3);
sats = gpsSol.numSat; sats = gpsSol.numSat;
if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) { if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) {
result = true; result = true;

View file

@ -137,7 +137,7 @@ static void ltm_gframe(void)
if (!STATE(GPS_FIX)) if (!STATE(GPS_FIX))
gps_fix_type = 1; gps_fix_type = 1;
else if (gpsSol.numSat < gpsConfig()->gpsMinimumSats) else if (gpsSol.numSat < GPS_MIN_SAT_COUNT)
gps_fix_type = 2; gps_fix_type = 2;
else else
gps_fix_type = 3; gps_fix_type = 3;

View file

@ -310,7 +310,7 @@ void mavlinkSendPosition(void)
gpsFixType = 1; gpsFixType = 1;
} }
else { else {
if (gpsSol.numSat < gpsConfig()->gpsMinimumSats) { if (gpsSol.numSat < GPS_MIN_SAT_COUNT) {
gpsFixType = 2; gpsFixType = 2;
} }
else { else {

View file

@ -296,7 +296,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint16_t altitudeLoBcd, groundCourseBcd, hdop;
uint8_t hdopBcd, gpsFlags; uint8_t hdopBcd, gpsFlags;
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < gpsConfig()->gpsMinimumSats) { if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < GPS_MIN_SAT_COUNT) {
return false; return false;
} }
@ -364,7 +364,7 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
uint8_t numSatBcd, altitudeHighBcd; uint8_t numSatBcd, altitudeHighBcd;
bool timeProvided = false; bool timeProvided = false;
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < gpsConfig()->gpsMinimumSats) { if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < GPS_MIN_SAT_COUNT) {
return false; return false;
} }

View file

@ -1193,8 +1193,6 @@ TEST_F(OsdTest, TestGpsElements)
{ {
// given // given
osdElementConfigMutable()->item_pos[OSD_GPS_SATS] = OSD_POS(2, 4) | OSD_PROFILE_1_FLAG; 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); sensorsSet(SENSOR_GPS);
osdAnalyzeActiveElements(); osdAnalyzeActiveElements();

View file

@ -22,26 +22,33 @@ extern "C" {
#include "build/debug.h" #include "build/debug.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/streambuf.h" #include "common/streambuf.h"
#include "config/feature.h" #include "config/feature.h"
#include "config/config.h" #include "config/config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/core.h" #include "fc/core.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "flight/gps_rescue.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "pg/motor.h" #include "pg/motor.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "pg/rx.h" #include "pg/rx.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
@ -61,7 +68,7 @@ extern "C" {
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_CONFIG, 0);
float rcCommand[4]; float rcCommand[4];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];