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

update GPS code

Browse source
This commit is contained in:
ctzsnooze 2022-04-25 13:08:26 +10:00
parent 229ac66755
commit f7c645bfcd
23 changed files with 671 additions and 490 deletions
Download patch file Download diff file Expand all files Collapse all files

39
src/main/blackbox/blackbox.c
View file

@ -62,6 +62,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.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"
@ -1509,6 +1510,44 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST, "%d", currentPidProfile->throttle_boost); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST, "%d", currentPidProfile->throttle_boost);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST_CUTOFF, "%d", currentPidProfile->throttle_boost_cutoff) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST_CUTOFF, "%d", currentPidProfile->throttle_boost_cutoff)
#ifdef USE_GPS
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)
#ifdef USE_GPS_RESCUE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ANGLE, "%d", gpsRescueConfig()->angle)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_BUFFER, "%d", gpsRescueConfig()->rescueAltitudeBufferM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_ALT, "%d", gpsRescueConfig()->initialAltitudeM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_DIST, "%d", gpsRescueConfig()->targetLandingDistanceM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_D, "%d", gpsRescueConfig()->throttleD)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate)
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)
#ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag)
#endif
#endif
#endif
default: default:
return true; return true;
} }

5
src/main/build/debug.c
View file

@ -99,5 +99,8 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"TIMING_ACCURACY", "TIMING_ACCURACY",
"RX_EXPRESSLRS_SPI", "RX_EXPRESSLRS_SPI",
"RX_EXPRESSLRS_PHASELOCK", "RX_EXPRESSLRS_PHASELOCK",
"RX_STATE_TIME" "RX_STATE_TIME",
"GPS_RESCUE_VELOCITY",
"GPS_RESCUE_HEADING",
"GPS_RESCUE_TRACKING"
}; };

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

@ -98,6 +98,9 @@ typedef enum {
DEBUG_RX_EXPRESSLRS_SPI, DEBUG_RX_EXPRESSLRS_SPI,
DEBUG_RX_EXPRESSLRS_PHASELOCK, DEBUG_RX_EXPRESSLRS_PHASELOCK,
DEBUG_RX_STATE_TIME, DEBUG_RX_STATE_TIME,
DEBUG_GPS_RESCUE_VELOCITY,
DEBUG_GPS_RESCUE_HEADING,
DEBUG_GPS_RESCUE_TRACKING,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

69
src/main/cli/settings.c
View file

@ -1007,45 +1007,46 @@ const clivalue_t valueTable[] = {
// PG_GPS_CONFIG // PG_GPS_CONFIG
#ifdef USE_GPS #ifdef USE_GPS
{ "gps_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) }, { PARAM_NAME_GPS_PROVIDER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) },
{ "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_SBAS_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbasMode) }, { PARAM_NAME_GPS_SBAS_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_SBAS_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbasMode) },
{ "gps_sbas_integrity", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) }, { PARAM_NAME_GPS_SBAS_INTEGRITY, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) },
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) }, { PARAM_NAME_GPS_AUTO_CONFIG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) },
{ "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, { PARAM_NAME_GPS_AUTO_BAUD, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) },
{ "gps_ublox_use_galileo", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) }, { PARAM_NAME_GPS_UBLOX_USE_GALILEO, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) },
{ "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) },
{ "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) },
{ "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) },
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
// PG_GPS_RESCUE // PG_GPS_RESCUE
{ "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, { PARAM_NAME_GPS_RESCUE_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
{ "gps_rescue_alt_buffer", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) }, { PARAM_NAME_GPS_RESCUE_ALT_BUFFER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) },
{ "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, { PARAM_NAME_GPS_RESCUE_INITIAL_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 2, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
{ "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) }, { PARAM_NAME_GPS_RESCUE_DESCENT_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
{ "gps_rescue_landing_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 10 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) }, { PARAM_NAME_GPS_RESCUE_LANDING_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) },
{ "gps_rescue_landing_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingDistanceM) }, { PARAM_NAME_GPS_RESCUE_LANDING_DIST, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingDistanceM) },
{ "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, { PARAM_NAME_GPS_RESCUE_GROUND_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
{ "gps_rescue_throttle_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, 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) },
{ "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },
{ "gps_rescue_throttle_d", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) },
{ "gps_rescue_velocity_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) }, { PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) },
{ "gps_rescue_velocity_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) }, { PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
{ "gps_rescue_velocity_d", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) }, { PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
{ "gps_rescue_yaw_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) }, { PARAM_NAME_GPS_RESCUE_YAW_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) },
{ PARAM_NAME_GPS_RESCUE_ROLL_MIX, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rollMix) },
{ "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
{ "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
{ "gps_rescue_ascend_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) }, { PARAM_NAME_GPS_RESCUE_ASCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) },
{ "gps_rescue_descend_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) }, { PARAM_NAME_GPS_RESCUE_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
{ "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) },
{ "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) },
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, { PARAM_NAME_GPS_RESCUE_MIN_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
{ "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, { PARAM_NAME_GPS_RESCUE_MIN_DTH, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
{ "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) },
{ "gps_rescue_alt_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) }, { 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) },
#ifdef USE_MAG #ifdef USE_MAG
{ "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) }, { PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
#endif #endif
#endif #endif
#endif #endif

10
src/main/cms/cms_menu_gps_rescue.c
View file

@ -175,18 +175,18 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
{"--- GPS RESCUE ---", OME_Label, NULL, NULL}, {"--- GPS RESCUE ---", OME_Label, NULL, NULL},
{ "ANGLE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,1 } }, { "ANGLE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,1 } },
{ "MIN DIST HOME M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 50, 1000 ,1 } }, { "MIN DIST HOME M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 25, 1000 ,1 } },
{ "INITAL ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 20, 100, 1 } }, { "INITAL ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 5, 100, 1 } },
{ "ALTITUDE MODE" , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} }, { "ALTITUDE MODE" , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} },
{ "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 30, 500, 1 } }, { "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 15, 500, 1 } },
{ "LANDING ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 10, 1 } }, { "LANDING ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 10, 1 } },
{ "LANDING DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingDistanceM, 5, 15, 1 } }, { "LANDING DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingDistanceM, 3, 15, 1 } },
{ "GROUND SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 30, 3000, 1 } }, { "GROUND SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 30, 3000, 1 } },
{ "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } }, { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } },
{ "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 } },
{ "ASCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 100, 2500, 1 } }, { "ASCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 100, 2500, 1 } },
{ "DESCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 100, 500, 1 } }, { "DESCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 50, 500, 1 } },
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, { "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){ &gpsRescueConfig_minSats, 5, 50, 1 } },
{ "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid}, { "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},

1
src/main/fc/core.c
View file

@ -961,7 +961,6 @@ void processRxModes(timeUs_t currentTimeUs)
} }
bool canUseHorizonMode = true; bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode // bumpless transfer to Level mode
canUseHorizonMode = false; canUseHorizonMode = false;

43
src/main/fc/parameter_names.h
View file

@ -117,3 +117,46 @@
#define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz" #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_FADE_RANGE_HZ "rpm_filter_fade_range_hz"
#define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz" #define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz"
#ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
#define PARAM_NAME_GPS_SBAS_MODE "gps_sbas_mode"
#define PARAM_NAME_GPS_SBAS_INTEGRITY "gps_sbas_integrity"
#define PARAM_NAME_GPS_AUTO_CONFIG "gps_auto_config"
#define PARAM_NAME_GPS_AUTO_BAUD "gps_auto_baud"
#define PARAM_NAME_GPS_UBLOX_USE_GALILEO "gps_ublox_use_galileo"
#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"
#ifdef USE_GPS_RESCUE
#define PARAM_NAME_GPS_RESCUE_ANGLE "gps_rescue_angle"
#define PARAM_NAME_GPS_RESCUE_ALT_BUFFER "gps_rescue_alt_buffer"
#define PARAM_NAME_GPS_RESCUE_INITIAL_ALT "gps_rescue_initial_alt"
#define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist"
#define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt"
#define PARAM_NAME_GPS_RESCUE_LANDING_DIST "gps_rescue_landing_dist"
#define PARAM_NAME_GPS_RESCUE_GROUND_SPEED "gps_rescue_ground_speed"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_I "gps_rescue_throttle_i"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_D "gps_rescue_throttle_d"
#define PARAM_NAME_GPS_RESCUE_VELOCITY_P "gps_rescue_velocity_p"
#define PARAM_NAME_GPS_RESCUE_VELOCITY_I "gps_rescue_velocity_i"
#define PARAM_NAME_GPS_RESCUE_VELOCITY_D "gps_rescue_velocity_d"
#define PARAM_NAME_GPS_RESCUE_YAW_P "gps_rescue_yaw_p"
#define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"
#define PARAM_NAME_GPS_RESCUE_ASCEND_RATE "gps_rescue_ascend_rate"
#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"
#ifdef USE_MAG
#define PARAM_NAME_GPS_RESCUE_USE_MAG "gps_rescue_use_mag"
#endif
#endif
#endif

3
src/main/fc/rc_controls.c
View file

@ -177,7 +177,8 @@ void processRcStickPositions()
resetTryingToArm(); resetTryingToArm();
// Disarming via ARM BOX // Disarming via ARM BOX
resetArmingDisabled(); resetArmingDisabled();
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { const bool switchFailsafe = (failsafeIsActive() && (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)));
if ((ARMING_FLAG(ARMED) && failsafeIsReceivingRxData()) || (ARMING_FLAG(ARMED) && switchFailsafe)) {
rcDisarmTicks++; rcDisarmTicks++;
if (rcDisarmTicks > 3) { if (rcDisarmTicks > 3) {
disarm(DISARM_REASON_SWITCH); disarm(DISARM_REASON_SWITCH);

31
src/main/flight/failsafe.c
View file

@ -101,7 +101,7 @@ void failsafeReset(void)
failsafeState.throttleLowPeriod = 0; failsafeState.throttleLowPeriod = 0;
failsafeState.landingShouldBeFinishedAt = 0; failsafeState.landingShouldBeFinishedAt = 0;
failsafeState.receivingRxDataPeriod = 0; failsafeState.receivingRxDataPeriod = 0;
failsafeState.receivingRxDataPeriodPreset = 0; failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
failsafeState.phase = FAILSAFE_IDLE; failsafeState.phase = FAILSAFE_IDLE;
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
} }
@ -142,6 +142,9 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
bool failsafeIsReceivingRxData(void) bool failsafeIsReceivingRxData(void)
{ {
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP); return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
// False with failsafe switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
// stays false until after recovery period expires
// Link down is the trigger for the various failsafe stage 2 outcomes.
} }
void failsafeOnRxSuspend(uint32_t usSuspendPeriod) void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
@ -174,8 +177,10 @@ void failsafeOnValidDataReceived(void)
// using the BST flag since no other suitable name.... // using the BST flag since no other suitable name....
} }
if (cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod){ if (cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.receivingRxDataPeriodPreset) {
// receivingRxDataPeriodPreset is rxDataRecoveryPeriod unless set to zero to allow immediate control recovery after switch induced failsafe
// rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms) // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms)
// link is not considered 'up', after it has been 'down', until that recovery period has expired
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
unsetArmingDisabled(ARMING_DISABLED_BST); unsetArmingDisabled(ARMING_DISABLED_BST);
} }
@ -219,7 +224,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
} }
bool receivingRxData = failsafeIsReceivingRxData(); bool receivingRxData = failsafeIsReceivingRxData();
// should be true when FAILSAFE_RXLINK_UP // true when FAILSAFE_RXLINK_UP
// FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived
// failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
@ -228,7 +233,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
beeperMode_e beeperMode = BEEPER_SILENCE; beeperMode_e beeperMode = BEEPER_SILENCE;
if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) { if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
// Aux switch set to failsafe stage2 emulates loss of signal without waiting // Aux switch set to failsafe stage2 emulates immediate loss of signal without waiting
failsafeOnValidDataFailed(); failsafeOnValidDataFailed();
receivingRxData = false; receivingRxData = false;
} }
@ -320,10 +325,14 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
failsafeState.phase = FAILSAFE_GPS_RESCUE; failsafeState.phase = FAILSAFE_GPS_RESCUE;
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// allow re-arming 3 seconds after Rx recovery // allow re-arming 1 second after Rx recovery
break; break;
#endif #endif
} }
if (failsafeSwitchIsOn) {
failsafeState.receivingRxDataPeriodPreset = 0;
// allow immediate recovery if failsafe is triggered by a switch
}
} }
reprocessState = true; reprocessState = true;
break; break;
@ -338,6 +347,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
} }
if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) { if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
// to manually disarm while Landing, aux channels must be enabled // to manually disarm while Landing, aux channels must be enabled
// note also that disarming via arm box must be possible during failsafe in rc_controls.c
// this should be blocked during signal not received periods, to avoid false disarms
// but should be allowed otherwise, eg after signal recovers, or during switch initiated failsafe
failsafeState.phase = FAILSAFE_LANDED; failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true; reprocessState = true;
} }
@ -346,7 +358,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
case FAILSAFE_GPS_RESCUE: case FAILSAFE_GPS_RESCUE:
if (receivingRxData) { if (receivingRxData) {
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || !failsafeSwitchIsOn) {
// this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale // this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true; reprocessState = true;
@ -373,7 +385,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
break; break;
case FAILSAFE_RX_LOSS_MONITORING: case FAILSAFE_RX_LOSS_MONITORING:
// Monitoring the rx link, allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. // receivingRxData is true when we get valid Rx Data and the recovery period has expired
// for switch initiated failsafes, the recovery period is zero
if (receivingRxData) { if (receivingRxData) {
if (millis() > failsafeState.receivingRxDataPeriod) { if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now // rx link is good now
@ -386,9 +399,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
break; break;
case FAILSAFE_RX_LOSS_RECOVERED: case FAILSAFE_RX_LOSS_RECOVERED:
// Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. // Entering IDLE, terminating failsafe, reset throttle low timer
// This is to prevent that JustDisarm is activated on the next iteration.
// Because that would have the effect of shutting down failsafe handling on intermittent connections.
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.phase = FAILSAFE_IDLE; failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false; failsafeState.active = false;

829
src/main/flight/gps_rescue.c
View file

File diff suppressed because it is too large Load diff

9
src/main/flight/gps_rescue.h
View file

@ -26,12 +26,12 @@ typedef struct gpsRescue_s {
uint16_t initialAltitudeM; //meters uint16_t initialAltitudeM; //meters
uint16_t descentDistanceM; //meters uint16_t descentDistanceM; //meters
uint16_t rescueGroundspeed; //centimeters per second uint16_t rescueGroundspeed; //centimeters per second
uint16_t throttleP, throttleI, throttleD; uint8_t throttleP, throttleI, throttleD;
uint16_t yawP; uint8_t yawP;
uint16_t throttleMin; uint16_t throttleMin;
uint16_t throttleMax; uint16_t throttleMax;
uint16_t throttleHover; uint16_t throttleHover;
uint16_t velP, velI, velD; uint8_t velP, velI, velD;
uint8_t minSats; uint8_t minSats;
uint16_t minRescueDth; //meters uint16_t minRescueDth; //meters
uint8_t sanityChecks; uint8_t sanityChecks;
@ -43,11 +43,12 @@ typedef struct gpsRescue_s {
uint16_t ascendRate; uint16_t ascendRate;
uint16_t descendRate; uint16_t descendRate;
uint16_t rescueAltitudeBufferM; //meters uint16_t rescueAltitudeBufferM; //meters
uint8_t rollMix;
} gpsRescueConfig_t; } gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
extern int32_t gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES
void updateGPSRescueState(void); void updateGPSRescueState(void);
void rescueNewGpsData(void); void rescueNewGpsData(void);

3
src/main/flight/imu.c
View file

@ -85,7 +85,8 @@ static bool imuUpdated = false;
#define ATTITUDE_RESET_GYRO_LIMIT 15 // 15 deg/sec - gyro limit for quiet period #define ATTITUDE_RESET_GYRO_LIMIT 15 // 15 deg/sec - gyro limit for quiet period
#define ATTITUDE_RESET_KP_GAIN 25.0 // dcmKpGain value to use during attitude reset #define ATTITUDE_RESET_KP_GAIN 25.0 // dcmKpGain value to use during attitude reset
#define ATTITUDE_RESET_ACTIVE_TIME 500000 // 500ms - Time to wait for attitude to converge at high gain #define ATTITUDE_RESET_ACTIVE_TIME 500000 // 500ms - Time to wait for attitude to converge at high gain
#define GPS_COG_MIN_GROUNDSPEED 500 // 500cm/s minimum groundspeed for a gps heading to be considered valid #define GPS_COG_MIN_GROUNDSPEED 200 // 200cm/s minimum groundspeed for a gps based IMU heading to be considered valid
// Better to have some update than none for GPS Rescue at slow return speeds
float accAverage[XYZ_AXIS_COUNT]; float accAverage[XYZ_AXIS_COUNT];

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

@ -81,13 +81,13 @@ static char *gpsPacketLogChar = gpsPacketLog;
// ********************** // **********************
int32_t GPS_home[2]; int32_t GPS_home[2];
uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees uint32_t GPS_distanceToHomeCm;
int16_t GPS_directionToHome; // direction to home or hol point in degrees * 10
uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s
float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
int16_t nav_takeoff_bearing; int16_t nav_takeoff_bearing;
#define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 0.54 km/h 0.335 mph
gpsSolutionData_t gpsSol; gpsSolutionData_t gpsSol;
uint32_t GPS_packetCount = 0; uint32_t GPS_packetCount = 0;
@ -1838,10 +1838,12 @@ void GPS_calculateDistanceAndDirectionToHome(void)
uint32_t dist; uint32_t dist;
int32_t dir; int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir); GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
GPS_distanceToHome = dist / 100; GPS_distanceToHome = dist / 100; // m/s
GPS_directionToHome = dir / 100; GPS_distanceToHomeCm = dist; // cm/sec
GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees
} else { } else {
GPS_distanceToHome = 0; GPS_distanceToHome = 0;
GPS_distanceToHomeCm = 0;
GPS_directionToHome = 0; GPS_directionToHome = 0;
} }
} }
@ -1852,16 +1854,6 @@ void onGpsNewData(void)
return; return;
} }
//
// Calculate time delta for navigation loop, range 0-1.0f, in seconds
//
// Time for calculating x,y speed and navigation pids
static uint32_t nav_loopTimer;
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = MIN(dTnav, 1.0f);
GPS_calculateDistanceAndDirectionToHome(); GPS_calculateDistanceAndDirectionToHome();
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
GPS_calculateDistanceFlownVerticalSpeed(false); GPS_calculateDistanceFlownVerticalSpeed(false);

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

@ -142,11 +142,11 @@ extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern int32_t GPS_home[2]; extern int32_t GPS_home[2];
extern uint16_t GPS_distanceToHome; // distance to home point in meters extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern uint32_t GPS_distanceToHomeCm; // distance to home point in cm
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
extern int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s extern int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
extern int16_t nav_takeoff_bearing; extern int16_t nav_takeoff_bearing;

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

@ -1023,7 +1023,7 @@ static void osdElementGpsHomeDirection(osdElementParms_t *element)
{ {
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
if (GPS_distanceToHome > 0) { if (GPS_distanceToHome > 0) {
const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw); const int h = DECIDEGREES_TO_DEGREES(GPS_directionToHome - attitude.values.yaw);
element->buff[0] = osdGetDirectionSymbolFromHeading(h); element->buff[0] = osdGetDirectionSymbolFromHeading(h);
} else { } else {
element->buff[0] = SYM_OVER_HOME; element->buff[0] = SYM_OVER_HOME;

1
src/main/rx/rx.c
View file

@ -674,6 +674,7 @@ void detectAndApplySignalLossBehaviour(void)
sample = getRxfailValue(channel); sample = getRxfailValue(channel);
// set channels to Stage 1 values immediately failsafe switch is activated // set channels to Stage 1 values immediately failsafe switch is activated
} else if (!thisChannelValid) { } else if (!thisChannelValid) {
// everything was normal and this channel was invalid
if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) { if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
// first 300ms of Stage 1 failsafe // first 300ms of Stage 1 failsafe
sample = rcData[channel]; sample = rcData[channel];

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

@ -144,7 +144,7 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
sbufWriteU8(dst, gpsSol.numSat); sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10)); // use units of 10m to increase range of U16 to 655.36km sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10)); // use units of 10m to increase range of U16 to 655.36km
sbufWriteU16(dst, GPS_directionToHome); sbufWriteU16(dst, GPS_directionToHome / 10);
uint8_t gpsFlags = 0; uint8_t gpsFlags = 0;
if (STATE(GPS_FIX)) { if (STATE(GPS_FIX)) {

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

@ -244,7 +244,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8; hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
hottGPSMessage->home_direction = GPS_directionToHome; hottGPSMessage->home_direction = GPS_directionToHome / 10;
} }
#endif #endif

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

@ -365,7 +365,7 @@ int32_t getSensorValue(uint8_t sensor)
break; break;
case EX_GPS_DIRECTION_TO_HOME: case EX_GPS_DIRECTION_TO_HOME:
return GPS_directionToHome; return GPS_directionToHome / 10;
break; break;
case EX_GPS_HEADING: case EX_GPS_HEADING:

5
src/test/unit/arming_prevention_unittest.cc
View file

@ -72,7 +72,7 @@ extern "C" {
bool cmsInMenu = false; bool cmsInMenu = false;
float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3]; float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
rxRuntimeState_t rxRuntimeState = {}; rxRuntimeState_t rxRuntimeState = {};
uint16_t GPS_distanceToHome = 0; uint32_t GPS_distanceToHomeCm = 0;
int16_t GPS_directionToHome = 0; int16_t GPS_directionToHome = 0;
acc_t acc = {}; acc_t acc = {};
bool mockIsUpright = false; bool mockIsUpright = false;
@ -1059,6 +1059,8 @@ extern "C" {
void failsafeStartMonitoring(void) {} void failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {} void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; } bool failsafeIsActive(void) { return false; }
bool failsafeIsReceivingRxData(void) { return false; }
bool rxAreFlightChannelsValid(void) { return false; }
void pidResetIterm(void) {} void pidResetIterm(void) {}
void updateAdjustmentStates(void) {} void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {} void processRcAdjustments(controlRateConfig_t *) {}
@ -1109,4 +1111,5 @@ extern "C" {
bool isMotorProtocolEnabled(void) { return true; } bool isMotorProtocolEnabled(void) { return true; }
void pinioBoxTaskControl(void) {} void pinioBoxTaskControl(void) {}
void schedulerSetNextStateTime(timeDelta_t) {} void schedulerSetNextStateTime(timeDelta_t) {}
float pt1FilterGain(float, float) {return 0.5f;}
} }

61
src/test/unit/flight_failsafe_unittest.cc
View file

@ -513,44 +513,15 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage1OrStage2Drop)
// deactivate the failsafe switch // deactivate the failsafe switch
deactivateBoxFailsafe(); deactivateBoxFailsafe();
// receivingRxData is immediately true
// we go directly to failsafe monitoring mode, via Landing
// because the switch also forces rxFlightChannelsValid false, emulating real failsafe
// we have two delays to deal with before we can re-arm
EXPECT_TRUE(failsafeIsActive()); EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled()); EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// handle the first delay in rxDataRecoveryPeriod // by next evaluation we should be out of failsafe
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// handle the second delay
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// one tick later
sysTickUptime ++; sysTickUptime ++;
// receivingRxData is immediately true because signal exists
failsafeOnValidDataReceived();
// when // when
failsafeUpdateState(); failsafeUpdateState();
@ -647,34 +618,10 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// handle the first delay in rxDataRecoveryPeriod
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// handle the second delay
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// one tick later // one tick later
sysTickUptime ++; sysTickUptime ++;
failsafeOnValidDataReceived();
// when // when
failsafeUpdateState(); failsafeUpdateState();

1
src/test/unit/rc_controls_unittest.cc
View file

@ -626,6 +626,7 @@ void dashboardEnablePageCycling() {}
bool failsafeIsActive() { return false; } bool failsafeIsActive() { return false; }
bool rxIsReceivingSignal() { return true; } bool rxIsReceivingSignal() { return true; }
bool failsafeIsReceivingRxData() { return true; }
uint8_t getCurrentControlRateProfileIndex(void) { uint8_t getCurrentControlRateProfileIndex(void) {
return 0; return 0;

2
src/test/unit/vtx_unittest.cc
View file

@ -152,6 +152,8 @@ extern "C" {
void failsafeStartMonitoring(void) {} void failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {} void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; } bool failsafeIsActive(void) { return false; }
bool rxAreFlightChannelsValid(void) { return false; }
bool failsafeIsReceivingRxData(void) { return false; }
void pidResetIterm(void) {} void pidResetIterm(void) {}
void updateAdjustmentStates(void) {} void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {} void processRcAdjustments(controlRateConfig_t *) {}