diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 552760f141..7ff6d162d2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -62,6 +62,7 @@ #include "flight/pid.h" #include "flight/rpm_filter.h" #include "flight/servos.h" +#include "flight/gps_rescue.h" #include "io/beeper.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_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: return true; } diff --git a/src/main/build/debug.c b/src/main/build/debug.c index d3ae5fb3f4..b855ad39e0 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -99,5 +99,8 @@ const char * const debugModeNames[DEBUG_COUNT] = { "TIMING_ACCURACY", "RX_EXPRESSLRS_SPI", "RX_EXPRESSLRS_PHASELOCK", - "RX_STATE_TIME" + "RX_STATE_TIME", + "GPS_RESCUE_VELOCITY", + "GPS_RESCUE_HEADING", + "GPS_RESCUE_TRACKING" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 8494550257..98ae5529c5 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -98,6 +98,9 @@ typedef enum { DEBUG_RX_EXPRESSLRS_SPI, DEBUG_RX_EXPRESSLRS_PHASELOCK, DEBUG_RX_STATE_TIME, + DEBUG_GPS_RESCUE_VELOCITY, + DEBUG_GPS_RESCUE_HEADING, + DEBUG_GPS_RESCUE_TRACKING, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 742219fb50..0541756039 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1007,45 +1007,46 @@ const clivalue_t valueTable[] = { // PG_GPS_CONFIG #ifdef USE_GPS - { "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) }, - { "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) }, - { "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) }, - { "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) }, - { "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_PROVIDER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) }, + { PARAM_NAME_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_INTEGRITY, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) }, + { PARAM_NAME_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_BAUD, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, + { 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) }, + { 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) }, #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE - { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, 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) }, - { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 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) }, - { "gps_rescue_landing_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 10 }, 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) }, - { "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 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) }, - { "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, 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) }, - { "gps_rescue_velocity_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, 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) }, - { "gps_rescue_velocity_d", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, 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_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, + { PARAM_NAME_GPS_RESCUE_ALT_BUFFER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) }, + { PARAM_NAME_GPS_RESCUE_INITIAL_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 2, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, + { PARAM_NAME_GPS_RESCUE_DESCENT_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) }, + { PARAM_NAME_GPS_RESCUE_LANDING_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) }, + { PARAM_NAME_GPS_RESCUE_LANDING_DIST, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingDistanceM) }, + { PARAM_NAME_GPS_RESCUE_GROUND_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, + { 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_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, + { PARAM_NAME_GPS_RESCUE_THROTTLE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) }, + { PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) }, + { PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) }, + { PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) }, + { 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) }, - { "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) }, - { "gps_rescue_descend_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 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) }, - { "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) }, - { "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 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) }, - { "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_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) }, + { PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) }, + { PARAM_NAME_GPS_RESCUE_ASCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) }, + { 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) }, #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 diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c index 627f23b6f2..bcce51f1b1 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/src/main/cms/cms_menu_gps_rescue.c @@ -175,18 +175,18 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = {"--- GPS RESCUE ---", OME_Label, NULL, NULL}, { "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 } }, - { "INITAL ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 20, 100, 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, 5, 100, 1 } }, { "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 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 } }, { "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 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 } }, - { "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 }, { "MIN SATELITES", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } }, { "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid}, diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 9e542d6726..77f51dae66 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -961,7 +961,6 @@ void processRxModes(timeUs_t currentTimeUs) } bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index ecadc67529..f351895ca1 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -117,3 +117,46 @@ #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" + +#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 diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index baa6458b9f..b0fa90e554 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -177,7 +177,8 @@ void processRcStickPositions() resetTryingToArm(); // Disarming via ARM BOX 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++; if (rcDisarmTicks > 3) { disarm(DISARM_REASON_SWITCH); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index b4230bd891..8779e8fcf4 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -101,7 +101,7 @@ void failsafeReset(void) failsafeState.throttleLowPeriod = 0; failsafeState.landingShouldBeFinishedAt = 0; failsafeState.receivingRxDataPeriod = 0; - failsafeState.receivingRxDataPeriodPreset = 0; + failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } @@ -142,6 +142,9 @@ static bool failsafeShouldHaveCausedLandingByNow(void) bool failsafeIsReceivingRxData(void) { 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) @@ -174,8 +177,10 @@ void failsafeOnValidDataReceived(void) // 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) + // link is not considered 'up', after it has been 'down', until that recovery period has expired failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; unsetArmingDisabled(ARMING_DISABLED_BST); } @@ -219,7 +224,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) } bool receivingRxData = failsafeIsReceivingRxData(); - // should be true when FAILSAFE_RXLINK_UP + // true when FAILSAFE_RXLINK_UP // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour @@ -228,7 +233,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) beeperMode_e beeperMode = BEEPER_SILENCE; 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(); receivingRxData = false; } @@ -320,10 +325,14 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); failsafeState.phase = FAILSAFE_GPS_RESCUE; failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; - // allow re-arming 3 seconds after Rx recovery + // allow re-arming 1 second after Rx recovery break; #endif } + if (failsafeSwitchIsOn) { + failsafeState.receivingRxDataPeriodPreset = 0; + // allow immediate recovery if failsafe is triggered by a switch + } } reprocessState = true; break; @@ -338,6 +347,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) } if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) { // 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; reprocessState = true; } @@ -346,7 +358,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) #ifdef USE_GPS_RESCUE case FAILSAFE_GPS_RESCUE: 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 failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; @@ -373,7 +385,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) break; 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 (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now @@ -386,9 +399,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) break; case FAILSAFE_RX_LOSS_RECOVERED: - // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. - // 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. + // Entering IDLE, terminating failsafe, reset throttle low timer failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index a90b5cb0c0..d48aed843b 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -62,11 +62,13 @@ typedef enum { RESCUE_IDLE, RESCUE_INITIALIZE, RESCUE_ATTAIN_ALT, - RESCUE_CROSSTRACK, - RESCUE_LANDING_APPROACH, + RESCUE_ROTATE, + RESCUE_FLY_HOME, + RESCUE_DESCENT, RESCUE_LANDING, RESCUE_ABORT, - RESCUE_COMPLETE + RESCUE_COMPLETE, + RESCUE_DO_NOTHING } rescuePhase_e; typedef enum { @@ -76,30 +78,40 @@ typedef enum { RESCUE_LOWSATS, RESCUE_CRASH_FLIP_DETECTED, RESCUE_STALLED, - RESCUE_TOO_CLOSE + RESCUE_TOO_CLOSE, + RESCUE_NO_HOME_POINT } rescueFailureState_e; typedef struct { - int32_t targetAltitudeCm; - int32_t targetGroundspeed; - uint8_t minAngleDeg; - uint8_t maxAngleDeg; - bool crosstrack; + float returnAltitudeCm; + float targetAltitudeCm; + uint16_t targetVelocityCmS; + int8_t minAnglePitchDeg; // can be negative + uint8_t maxAnglePitchDeg; // must be positive + int8_t maxAngleRollDeg; // only one value since must always have a negative to positive range + bool updateYaw; + float descentDistanceM; } rescueIntent_s; typedef struct { int32_t maxAltitudeCm; int32_t currentAltitudeCm; - uint16_t distanceToHomeM; + float distanceToHomeCm; + float distanceToHomeM; uint16_t maxDistanceToHomeM; + uint16_t groundSpeedCmS; //cm/s int16_t directionToHome; - uint16_t groundSpeed; uint8_t numSat; - float zVelocity; // Up/down movement in cm/s - float zVelocityAvg; // Up/down average in cm/s float accMagnitude; - float accMagnitudeAvg; bool healthy; + float errorAngle; + float gpsDataIntervalSeconds; + float velocityToHomeCmS; + float ascendStepCm; + float descendStepCm; + int16_t distanceDescendedCm; + float maxPitchStep; + float filterK; } rescueSensorData_s; typedef struct { @@ -112,7 +124,6 @@ typedef struct { rescueFailureState_e failure; rescueSensorData_s sensor; rescueIntent_s intent; - bool isFailsafe; bool isAvailable; } rescueState_s; @@ -122,27 +133,11 @@ typedef enum { CURRENT_ALT } altitudeMode_e; -typedef struct { - float Kp; - float Ki; - float Kd; -} throttle_s; - -#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate -#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle -#define GPS_RESCUE_SLOWDOWN_DISTANCE_M 200 // distance from home to start decreasing speed -#define GPS_RESCUE_MIN_DESCENT_DIST_M 30 // minimum descent distance allowed -#define GPS_RESCUE_ZVELOCITY_THRESHOLD 300 // altitude threshold for start decreasing z velocity -#define GPS_RESCUE_LANDING_ZVELOCITY 80 // descend velocity for final landing phase -#define GPS_RESCUE_ITERM_WINDUP 100 // reset I term after z velocity error of 100 cm/s -#define GPS_RESCUE_MAX_ITERM_ACC 250.0f //max allowed iterm value -#define GPS_RESCUE_SLOWDOWN_ALT 500 // the altitude after which the quad begins to slow down the descend velocity -#define GPS_RESCUE_MINIMUM_ZVELOCITY 50 // minimum speed for final landing phase -#define GPS_RESCUE_ALMOST_LANDING_ALT 100 // altitude after which the quad increases ground detection sensitivity - -#define GPS_RESCUE_THROTTLE_P_SCALE 0.0003125f // pid scaler for P term -#define GPS_RESCUE_THROTTLE_I_SCALE 0.1f // pid scaler for I term -#define GPS_RESCUE_THROTTLE_D_SCALE 0.0003125f // pid scaler for D term +#define GPS_RESCUE_MAX_YAW_RATE 90 // deg/sec max yaw rate +#define GPS_RESCUE_MIN_DESCENT_DIST_M 10 // minimum descent distance allowed +#define GPS_RESCUE_MAX_ITERM_VELOCITY 1000 // max allowed iterm value for velocity +#define GPS_RESCUE_MAX_ITERM_THROTTLE 200 // max allowed iterm value for throttle +#define GPS_RESCUE_MAX_PITCH_RATE 3000 // max allowed change in pitch per second in degrees * 100 #ifdef USE_MAG #define GPS_RESCUE_USE_MAG true @@ -154,50 +149,44 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCU PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .angle = 32, - .initialAltitudeM = 50, - .descentDistanceM = 200, - .rescueGroundspeed = 2000, - .throttleP = 150, - .throttleI = 20, - .throttleD = 50, - .velP = 80, + .initialAltitudeM = 30, + .descentDistanceM = 20, + .rescueGroundspeed = 500, + .throttleP = 18, + .throttleI = 40, + .throttleD = 13, + .velP = 6, .velI = 20, - .velD = 15, - .yawP = 40, + .velD = 70, + .yawP = 25, .throttleMin = 1100, .throttleMax = 1600, - .throttleHover = 1280, - .sanityChecks = RESCUE_SANITY_ON, + .throttleHover = 1275, + .sanityChecks = RESCUE_SANITY_FS_ONLY, .minSats = 8, - .minRescueDth = 100, + .minRescueDth = 30, .allowArmingWithoutFix = false, .useMag = GPS_RESCUE_USE_MAG, .targetLandingAltitudeM = 5, - .targetLandingDistanceM = 10, + .targetLandingDistanceM = 5, .altitudeMode = MAX_ALT, - .ascendRate = 500, - .descendRate = 150, - .rescueAltitudeBufferM = 15, + .ascendRate = 500, // cm/s, for altitude corrections on ascent + .descendRate = 125, // cm/s, for descent and landing phase, or negative ascent + .rescueAltitudeBufferM = 10, + .rollMix = 100 ); -static uint16_t rescueThrottle; -static float rescueYaw; - -int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; -uint16_t hoverThrottle = 0; -float averageThrottle = 0.0; -float altitudeError = 0.0; -uint32_t throttleSamples = 0; -bool magForceDisable = false; - +static float rescueThrottle; +static float rescueYaw; +float velocityToHomeCmS; +float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; +bool magForceDisable = false; static bool newGPSData = false; rescueState_s rescueState; -throttle_s throttle; /* - If we have new GPS data, update home heading - if possible and applicable. + If we have new GPS data, update home heading if possible and applicable. */ void rescueNewGpsData(void) { @@ -214,7 +203,7 @@ static void rescueStop() rescueState.phase = RESCUE_IDLE; } -// Things that need to run regardless of GPS rescue mode being enabled or not +// Things that need to run regardless of GPS rescue mode being enabled or not (?? huh ?? ct) static void idleTasks() { // Do not calculate any of the idle task values when we are not flying @@ -229,164 +218,230 @@ static void idleTasks() return; } - gpsRescueAngle[AI_PITCH] = 0; - gpsRescueAngle[AI_ROLL] = 0; - // Store the max altitude we see not during RTH so we know our fly-back minimum alt rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm); // Store the max distance to home during normal flight so we know if a flyaway is happening - rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM); + rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.maxDistanceToHomeM, rescueState.sensor.distanceToHomeCm / 100.0f); - rescueThrottle = rcCommand[THROTTLE]; - - //to do: have a default value for hoverThrottle - - // FIXME: GPS Rescue throttle handling should take into account min_check as the - // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this - // in gpsRescueGetThrottle() but it would be better handled here. - - const float ct = getCosTiltAngle(); - if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt - //TO DO: only sample when acceleration is low - uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct; - if (throttleSamples == 0) { - averageThrottle = adjustedThrottle; - } else { - averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1); + // Keep the descent distance and intended altitude up to date with latest GPS values, to be available immediately when needed + if (newGPSData) { + rescueState.intent.descentDistanceM = constrainf(rescueState.sensor.distanceToHomeM, GPS_RESCUE_MIN_DESCENT_DIST_M, gpsRescueConfig()->descentDistanceM); + const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f; + const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f; + switch (gpsRescueConfig()->altitudeMode) { + case FIXED_ALT: + rescueState.intent.returnAltitudeCm = initialAltitudeCm; + break; + case CURRENT_ALT: + rescueState.intent.returnAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueAltitudeBufferCm; + break; + case MAX_ALT: + default: + rescueState.intent.returnAltitudeCm = rescueState.sensor.maxAltitudeCm + rescueAltitudeBufferCm; + break; } - hoverThrottle = lrintf(averageThrottle); - throttleSamples++; - } -} -// Very similar to maghold function on betaflight/cleanflight -static void setBearing(int16_t desiredHeading) -{ - float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading; - - // Determine the most efficient direction to rotate - if (errorAngle <= -180) { - errorAngle += 360; - } else if (errorAngle > 180) { - errorAngle -= 360; + // set the target altitude and velocity to current values, so there will be no D kick on first run + rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm; + rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS; + // set the landingInitiationAltitude for sanity check of Landing Mode. } - errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - - // Calculate a desired yaw rate based on a maximum limit beyond - // an error window and then scale the requested rate down inside - // the window as error approaches 0. - rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE); + rescueThrottle = rcCommand[THROTTLE]; // value to be returned when idle = normal throttle TODO ?? is this needed ?? } static void rescueAttainPosition() { - // Speed and altitude controller internal variables - static int16_t previousSpeedError = 0; - static int16_t speedIntegral = 0; - static int previousZVelocityError = 0; - static float zVelocityIntegral = 0; - static int16_t altitudeAdjustment = 0; + // runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase. + static float previousVelocityError = 0.0f; + static float velocityI = 0.0f; + static float previousVelocityD = 0.0f; // for smoothing + static float previousPitchAdjustment = 0.0f; + static float previousAltitudeError = 0.0f; + static float throttleI = 0.0f; + static float previousThrottleD = 0.0f; // for jerk calc from raw Derivative + static float previousThrottleDVal = 0.0f; // for moving average of D and jerk + static float previousThrottleD2 = 0.0f; // for additional D first order smoothing + static int16_t throttleAdjustment = 0; if (rescueState.phase == RESCUE_INITIALIZE) { // Initialize internal variables each time GPS Rescue is started - previousSpeedError = 0; - speedIntegral = 0; - previousZVelocityError = 0; - zVelocityIntegral = 0; - altitudeAdjustment = 0; + // Note valid sensor data can't be used here. Use idleTasks() for those kind of values + previousVelocityError = 0.0f; + velocityI = 0.0f; + previousVelocityD = 0.0f; + previousPitchAdjustment = 0.0f; + previousAltitudeError = 0.0f; + throttleI = 0.0f; + previousThrottleD = 0.0f; + previousThrottleDVal = 0.0f; + previousThrottleD2 = 0.0f; + throttleAdjustment = 0; + velocityToHomeCmS = 0.0f; + gpsRescueAngle[AI_ROLL] = 0.0f; + gpsRescueAngle[AI_PITCH] = 0.0f; + return; } - // Point to home if that is in our intent - if (rescueState.intent.crosstrack) { - setBearing(rescueState.sensor.directionToHome); - } - - DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data + if (rescueState.phase == RESCUE_DO_NOTHING) { + gpsRescueAngle[AI_PITCH] = 0.0f; + gpsRescueAngle[AI_ROLL] = 0.0f; + rescueThrottle = gpsRescueConfig()->throttleHover; + return; + } if (!newGPSData) { return; } - /** - Speed controller - */ - const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100; - const int16_t speedDerivative = speedError - previousSpeedError; - - speedIntegral = constrain(speedIntegral + speedError, -100, 100); - - previousSpeedError = speedError; - - const int16_t angleAdjustment = gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 + gpsRescueConfig()->velD * speedDerivative; - - gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100); - - const float ct = cosf(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); + const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f; /** - Altitude controller + Heading / yaw controller */ - const int16_t altitudeError = rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm; + // directionToHome and distanceToHome are accurate if the GPS Home point is accurate. + // attitude.values.yaw is set by imuCalculateEstimatedAttitude() but only updated while groundspeed exceeds 2 m/s + // for accurate return, the craft should exceed 5m/s in clean nose-forward flight at some point + // the faster the return speed, the more accurate the IMU will be, but the consequences of IMU error at the start are greater + // A compass (magnetometer) is vital for accurate GPS rescue at slow speeds + + // if the quad is pointing 180 degrees wrong at failsafe time, it will take 2s to rotate fully at 90 deg/s max rate + // this gives the level mode controller time to adjust pitch and roll during the yaw + // we need a relatively gradual trajectory change for attitude.values.yaw to update effectively + + rescueYaw = constrainf(rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * 0.1f, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE); + // rescueYaw is the yaw rate in deg/s to correct the heading error + + const float rollMixAttenuator = constrainf(1.0f - ABS(rescueYaw) * 0.01f, 0.0f, 1.0f); + // attenuate roll as yaw rate increases, no roll at 100 deg/s of yaw + const float rollAdjustment = - rescueYaw * gpsRescueConfig()->rollMix * rollMixAttenuator; + // mix in the desired amount of roll; 1:1 yaw:roll when rollMix = 100 and yaw angles are small + // when gpsRescueConfig()->rollMix is zero, there is no roll adjustment + // rollAdjustment is degrees * 100 + // note that the roll element has the same sign as the yaw element *before* GET_DIRECTION + gpsRescueAngle[AI_ROLL] = constrainf(rollAdjustment, rescueState.intent.maxAngleRollDeg * -100.0f, rescueState.intent.maxAngleRollDeg * 100.0f); + // gpsRescueAngle is added to the normal roll Angle Mode corrections in pid.c + + rescueYaw *= GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); + if (!rescueState.intent.updateYaw) { + rescueYaw = 0.0f; + } + + DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 0, rescueYaw * 10.0f); // Rescue yaw rate in degrees/sec * 10 + DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 1, gpsRescueAngle[AI_ROLL]); // roll correction degrees * 100 + + /** + Pitch / velocity controller + */ + float velocityError = (rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS); + // velocityError is in cm per second, positive means too slow. + // NB positive pitch setpoint means nose down. + // quite heavily smoothed + // IdleTasks sets target velocity to current velocity to minimise D spike when starting, and keep error = 0 // P component - float scalingRate; - if (ABS(altitudeError) > 0 && ABS(altitudeError) < GPS_RESCUE_ZVELOCITY_THRESHOLD) { - scalingRate = (float)altitudeError / GPS_RESCUE_ZVELOCITY_THRESHOLD; - } else { - scalingRate = 1; - } - - int zVelocityError; - if (altitudeError > 0) { - zVelocityError = gpsRescueConfig()->ascendRate * scalingRate - rescueState.sensor.zVelocity; - } else if (altitudeError < 0) { - if (rescueState.sensor.currentAltitudeCm <= GPS_RESCUE_SLOWDOWN_ALT) { - const int16_t rescueLandingDescendVel = MAX(GPS_RESCUE_LANDING_ZVELOCITY * rescueState.sensor.currentAltitudeCm / GPS_RESCUE_SLOWDOWN_ALT, GPS_RESCUE_MINIMUM_ZVELOCITY); - zVelocityError = -rescueLandingDescendVel - rescueState.sensor.zVelocity; - } else { - zVelocityError = -gpsRescueConfig()->descendRate * scalingRate - rescueState.sensor.zVelocity; - } - } else { - zVelocityError = 0; - } + const float velocityP = velocityError * gpsRescueConfig()->velP; // I component - if (ABS(zVelocityError) < GPS_RESCUE_ITERM_WINDUP) { - zVelocityIntegral = constrainf(zVelocityIntegral + zVelocityError / 100.0f, -GPS_RESCUE_MAX_ITERM_ACC, GPS_RESCUE_MAX_ITERM_ACC); - } else { - zVelocityIntegral = 0; - } + velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor; + // normalisation increases amount added when data rate is slower than expected + velocityI *= rescueState.intent.targetVelocityCmS / rescueState.intent.targetVelocityCmS; + // attenuate iTerm at slower target velocity, to minimise overshoot, mostly during deceleration to landing phase + velocityI = constrainf(velocityI, -1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY, 1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY); + // I component alone cannot exceed a pitch angle of 10% // D component - const int zVelocityDerivative = zVelocityError - previousZVelocityError; - previousZVelocityError = zVelocityError; + float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor); + previousVelocityError = velocityError; + // simple first order filter on derivative with k = 0.5 for 200ms steps + velocityD = previousVelocityD + rescueState.sensor.filterK * (velocityD - previousVelocityD); + previousVelocityD = velocityD; + velocityD *= gpsRescueConfig()->velD; - const int16_t hoverAdjustment = (hoverThrottle - 1000) / ct; - altitudeAdjustment = constrain(altitudeAdjustment + (throttle.Kp * zVelocityError + throttle.Ki * zVelocityIntegral + throttle.Kd * zVelocityDerivative), - gpsRescueConfig()->throttleMin - 1000 - hoverAdjustment, gpsRescueConfig()->throttleMax - 1000 - hoverAdjustment); + // Pitch PIDsum and smoothing + float pitchAdjustment = velocityP + velocityD + velocityI; + // simple rate of change limiter - not more than 25 deg/s of pitch change to keep pitch smooth + float pitchAdjustmentDelta = pitchAdjustment - previousPitchAdjustment; + if (pitchAdjustmentDelta > rescueState.sensor.maxPitchStep) { + pitchAdjustment = previousPitchAdjustment + rescueState.sensor.maxPitchStep; + } else if (pitchAdjustmentDelta < -rescueState.sensor.maxPitchStep) { + pitchAdjustment = previousPitchAdjustment - rescueState.sensor.maxPitchStep; + } + const float movingAvgPitchAdjustment = 0.5f * (previousPitchAdjustment + pitchAdjustment); + // moving average seems to work best here, a lot of sequential up and down in velocity data + previousPitchAdjustment = pitchAdjustment; + pitchAdjustment = movingAvgPitchAdjustment; + // pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100 + // it gets added to the normal level mode Pitch adjustments in pid.c - rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); + gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustment, rescueState.intent.minAnglePitchDeg * 100.0f, rescueState.intent.maxAnglePitchDeg * 100.0f); + // this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint - DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); - DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); - DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); + DEBUG_SET(DEBUG_RTH, 0, gpsRescueAngle[AI_PITCH]); + DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, velocityP); + DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, velocityD); + DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, rescueState.intent.targetVelocityCmS); + DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 1, rescueState.intent.targetVelocityCmS); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttle.Kp * zVelocityError); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttle.Ki * zVelocityIntegral); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, throttle.Kd * zVelocityDerivative); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.sensor.zVelocity); + /** + Altitude (throttle) controller + */ + // note that currentAltitudeCm can be updated more frequently than GPS rate from Baro, but this code requires GPS data to update + // ToDo: use a delta time for changes in currentAltitudeCm, and run more frequently than GPS rate + const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f; + // height above target in metres (negative means too low) + // at the start, the target starts at current altitude plus one step. Increases stepwise to intended value. + + // P component + const float throttleP = gpsRescueConfig()->throttleP * altitudeError; + + // I component + throttleI += 0.01f * gpsRescueConfig()->throttleI * altitudeError * sampleIntervalNormaliseFactor; + throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE); + // up to 20% increase in throttle from I alone + + // D component + // is error based, so includes positive boost when climbing and negative boost on descent + float throttleD = ((altitudeError - previousAltitudeError) / sampleIntervalNormaliseFactor); + previousAltitudeError = altitudeError; + + // Acceleration (Jerk) component + const float throttleDJerk = 2.0f * (throttleD - previousThrottleD); + previousThrottleD = throttleD; + throttleD += throttleDJerk; + + // D Smoothing + const float movingAvgAltitudeD = 0.5f * (previousThrottleDVal + throttleD); + // moving average seems to work best here, a lot of sequential up and down in altitude data + previousThrottleDVal = throttleD; + throttleD = movingAvgAltitudeD; + throttleD = previousThrottleD2 + rescueState.sensor.filterK * (throttleD - previousThrottleD2); + // additional final first order D throttle smoothing + previousThrottleD2 = throttleD; + + throttleD = 10.0f * gpsRescueConfig()->throttleD * throttleD; + + float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day + tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000); + // if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful + // too much and landings with lots of pitch adjustment, eg windy days, can be a problem + + throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment; + + rescueThrottle = gpsRescueConfig()->throttleHover + throttleAdjustment; + rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD); } static void performSanityChecks() { - static uint32_t previousTimeUs = 0; // Last time Stalled/LowSat was checked + static timeUs_t previousTimeUs = 0; // Last time Stalled/LowSat was checked static int8_t secondsStalled = 0; // Stalled movement detection - static uint16_t lastDistanceToHomeM = 0; // Fly Away detection - static int8_t secondsFlyingAway = 0; + static int8_t secondsNotLanding = 0; // Stalled landing detection static int8_t secondsLowSats = 0; // Minimum sat detection - - const uint32_t currentTimeUs = micros(); + static int8_t secondsDoingNothing = 0; // Limit on doing nothing + const timeUs_t currentTimeUs = micros(); if (rescueState.phase == RESCUE_IDLE) { rescueState.failure = RESCUE_HEALTHY; @@ -394,22 +449,28 @@ static void performSanityChecks() } else if (rescueState.phase == RESCUE_INITIALIZE) { // Initialize internal variables each time GPS Rescue is started previousTimeUs = currentTimeUs; - secondsStalled = 10; // Start the count at 10 to be less forgiving at the beginning - lastDistanceToHomeM = rescueState.sensor.distanceToHomeM; - secondsFlyingAway = 0; + secondsStalled = 5; // Start the count at 5 to be less forgiving at the beginning + secondsNotLanding = 0; secondsLowSats = 5; // Start the count at 5 to be less forgiving at the beginning + secondsDoingNothing = 0; return; } - // Do not abort until each of these items is fully tested + // Handle rescue failures. Don't disarm for rescue failure during stick induced rescues. + const bool hardFailsafe = !rxIsReceivingSignal(); if (rescueState.failure != RESCUE_HEALTHY) { - if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON - || (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) { + if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON) { rescueState.phase = RESCUE_ABORT; + } else if ((gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY) && hardFailsafe) { + rescueState.phase = RESCUE_ABORT; + } else { + rescueState.phase = RESCUE_DO_NOTHING; } } - // Check if crash recovery mode is active, disarm if so. + DEBUG_SET(DEBUG_RTH, 2, rescueState.failure); + + // Check if crash recovery mode is active if (crashRecoveryModeActive()) { rescueState.failure = RESCUE_CRASH_FLIP_DETECTED; } @@ -420,73 +481,117 @@ static void performSanityChecks() } // Things that should run at a low refresh rate (such as flyaway detection, etc) - // This runs at ~1hz - const uint32_t dTime = currentTimeUs - previousTimeUs; + // This runs at 1hz + const timeDelta_t dTime = cmpTimeUs(currentTimeUs, previousTimeUs); if (dTime < 1000000) { //1hz return; } - previousTimeUs = currentTimeUs; - if (rescueState.phase == RESCUE_CROSSTRACK) { - secondsStalled = constrain(secondsStalled + ((rescueState.sensor.groundSpeed < 150) ? 1 : -1), 0, 20); - + if (rescueState.phase == RESCUE_FLY_HOME) { + secondsStalled = constrain(secondsStalled + ((rescueState.sensor.velocityToHomeCmS < (0.5 * rescueState.intent.targetVelocityCmS)) ? 1 : -1), 0, 20); if (secondsStalled == 20) { - rescueState.failure = RESCUE_STALLED; - } - - secondsFlyingAway = constrain(secondsFlyingAway + ((lastDistanceToHomeM < rescueState.sensor.distanceToHomeM) ? 1 : -1), 0, 10); - lastDistanceToHomeM = rescueState.sensor.distanceToHomeM; - - if (secondsFlyingAway == 10) { #ifdef USE_MAG //If there is a mag and has not been disabled, we have to assume is healthy and has been used in imu.c if (sensors(SENSOR_MAG) && gpsRescueConfig()->useMag && !magForceDisable) { //Try again with mag disabled magForceDisable = true; - secondsFlyingAway = 0; + secondsStalled = 0; } else #endif { - rescueState.failure = RESCUE_FLYAWAY; + rescueState.failure = RESCUE_STALLED; } } } - secondsLowSats = constrain(secondsLowSats + ((rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1), 0, 10); + + // These conditions are 'special', in that even with sanity checks off, they should still apply + if (rescueState.phase == RESCUE_LANDING) { + secondsNotLanding = constrain(secondsNotLanding + ((rescueState.sensor.distanceDescendedCm > (0.5f * rescueState.sensor.descendStepCm)) ? -1 : 1), 0, 10); + if (secondsNotLanding == 10) { + { + rescueState.phase = RESCUE_ABORT; + // if we lose the altitude signal, or fail to meet impact criteria, force terminate Landing Mode + } + } + } else if (rescueState.phase == RESCUE_DO_NOTHING) { + secondsDoingNothing = MIN(secondsDoingNothing + 1, 10); + if (secondsDoingNothing == 10) { + rescueState.phase = RESCUE_ABORT; + // prevent indefinite flyaways when sanity checks are off, and + // time limit the "do nothing" period when a switch initiated failsafe fails sanity checks + // this is controversial + } + } + + secondsLowSats = constrain(secondsLowSats + ((rescueState.sensor.numSat < (gpsRescueConfig()->minSats / 2)) ? 1 : -1), 0, 10); if (secondsLowSats == 10) { rescueState.failure = RESCUE_LOWSATS; } + + DEBUG_SET(DEBUG_RTH, 3, (secondsStalled * 100 + secondsLowSats)); //Failure can change with no new GPS Data } static void sensorUpdate() { + static timeUs_t previousDataTimeUs = 0; + static float prevDistanceToHomeCm = 0.0f; + static int32_t prevAlitudeCm = 0.0f; rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm(); + DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, rescueState.sensor.currentAltitudeCm); + // may be updated more frequently than GPS data + rescueState.sensor.healthy = gpsIsHealthy(); - // Calculate altitude velocity - static uint32_t previousTimeUs; - static int32_t previousAltitudeCm; - - const uint32_t currentTimeUs = micros(); - const float dTime = currentTimeUs - previousTimeUs; - - if (newGPSData) { // Calculate velocity at lowest common denominator - rescueState.sensor.distanceToHomeM = GPS_distanceToHome; - rescueState.sensor.directionToHome = GPS_directionToHome; - rescueState.sensor.numSat = gpsSol.numSat; - rescueState.sensor.groundSpeed = gpsSol.groundSpeed; - - rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime; - rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; - + if (rescueState.phase == RESCUE_LANDING) { + // do this at sensor update rate, not the much slower GPS rate, for quick disarm rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec; - rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f); - - previousAltitudeCm = rescueState.sensor.currentAltitudeCm; - previousTimeUs = currentTimeUs; } + + if (!newGPSData) { + return; + } + + rescueState.sensor.distanceToHomeCm = GPS_distanceToHomeCm; + 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) { + rescueState.sensor.errorAngle += 360; + } else if (rescueState.sensor.errorAngle > 180) { + rescueState.sensor.errorAngle -= 360; + } + + const timeUs_t currentTimeUs = micros(); + const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousDataTimeUs); + rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f); + // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values. + previousDataTimeUs = currentTimeUs; + + rescueState.sensor.filterK = pt1FilterGain(0.8, rescueState.sensor.gpsDataIntervalSeconds); + // 0.8341 for 1hz, 0.5013 for 5hz, 0.3345 for 10hz, 0.1674 for 25Hz, etc + + rescueState.sensor.velocityToHomeCmS = (prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds; + // positive = towards home. First value is useless since prevDistanceToHomeCm was zero. + prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm; + + rescueState.sensor.ascendStepCm = rescueState.sensor.gpsDataIntervalSeconds * gpsRescueConfig()->ascendRate; + rescueState.sensor.descendStepCm = rescueState.sensor.gpsDataIntervalSeconds * gpsRescueConfig()->descendRate; + rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE; + rescueState.sensor.distanceDescendedCm = prevAlitudeCm - rescueState.sensor.currentAltitudeCm; + prevAlitudeCm = rescueState.sensor.currentAltitudeCm; + + + DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10 + DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 3, rescueState.sensor.directionToHome); // degrees * 10 + DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, rescueState.sensor.velocityToHomeCmS); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm); + DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, rescueState.sensor.velocityToHomeCmS); } // This function checks the following conditions to determine if GPS rescue is available: @@ -497,8 +602,8 @@ static void sensorUpdate() // is also independent of the gps_rescue_sanity_checks configuration static bool checkGPSRescueIsAvailable(void) { - static uint32_t previousTimeUs = 0; // Last time LowSat was checked - const uint32_t currentTimeUs = micros(); + static timeUs_t previousTimeUs = 0; // Last time LowSat was checked + const timeUs_t currentTimeUs = micros(); static int8_t secondsLowSats = 0; // Minimum sat detection static bool lowsats = false; static bool noGPSfix = false; @@ -509,7 +614,7 @@ static bool checkGPSRescueIsAvailable(void) } // Things that should run at a low refresh rate >> ~1hz - const uint32_t dTime = currentTimeUs - previousTimeUs; + const timeDelta_t dTime = cmpTimeUs(currentTimeUs, previousTimeUs); if (dTime < 1000000) { //1hz if (noGPSfix || lowsats) { result = false; @@ -537,169 +642,197 @@ static bool checkGPSRescueIsAvailable(void) return result; } -/* - Determine what phase we are in, determine if all criteria are met to move to the next phase -*/ void updateGPSRescueState(void) +// this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active { - static uint16_t newDescentDistanceM; - static float_t lineSlope; - static float_t lineOffsetM; - static int32_t newSpeed; - static int32_t newAltitude; - float magnitudeTrigger; - if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { - rescueStop(); + rescueStop(); // sets phase to idle and does nothing else } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) { - rescueStart(); - rescueAttainPosition(); // Initialize - performSanityChecks(); // Initialize + rescueStart(); // sets phase to initialise if we enter GPS Rescue mode while idle + rescueAttainPosition(); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably) + performSanityChecks(); // Initialises sanity check values when a Rescue starts } - rescueState.isFailsafe = failsafeIsActive(); + // Will now be in INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE - sensorUpdate(); + sensorUpdate(); // always get latest GPS / Altitude data + uint8_t halfAngle = gpsRescueConfig()->angle / 2; + bool startedLow = true; rescueState.isAvailable = checkGPSRescueIsAvailable(); switch (rescueState.phase) { case RESCUE_IDLE: + // in Idle phase = NOT in GPS Rescue + // set maxAltitude for flight + // update the return altitude and descent distance values, to have valid settings immediately they are needed + // initialise the target altitude and velocity to current values to minimise D spike on startup idleTasks(); break; + // sanity checks are bypassed in IDLE mode; instead, failure state is always initialised to HEALTHY + // target altitude is always set to current altitude. + case RESCUE_INITIALIZE: - if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default. - hoverThrottle = gpsRescueConfig()->throttleHover; - } - - throttle.Kp = gpsRescueConfig()->throttleP * GPS_RESCUE_THROTTLE_P_SCALE; - throttle.Ki = gpsRescueConfig()->throttleI * GPS_RESCUE_THROTTLE_I_SCALE; - throttle.Kd = gpsRescueConfig()->throttleD * GPS_RESCUE_THROTTLE_D_SCALE; - + // Things that should abort the start of a Rescue if (!STATE(GPS_FIX_HOME)) { - setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); - disarm(DISARM_REASON_GPS_RESCUE); - } - - // Minimum distance detection. - if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { - rescueState.failure = RESCUE_TOO_CLOSE; - - // Never allow rescue mode to engage as a failsafe when too close. - if (rescueState.isFailsafe) { - setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); - disarm(DISARM_REASON_GPS_RESCUE); - } - - // When not in failsafe mode: leave it up to the sanity check setting. - } - - newSpeed = gpsRescueConfig()->rescueGroundspeed; - //set new descent distance if actual distance to home is lower - if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) { - newDescentDistanceM = MAX(rescueState.sensor.distanceToHomeM - 5, GPS_RESCUE_MIN_DESCENT_DIST_M); - } else { - newDescentDistanceM = gpsRescueConfig()->descentDistanceM; - } - - switch (gpsRescueConfig()->altitudeMode) { - case FIXED_ALT: - newAltitude = gpsRescueConfig()->initialAltitudeM * 100; - break; - case CURRENT_ALT: - newAltitude = rescueState.sensor.currentAltitudeCm + gpsRescueConfig()->rescueAltitudeBufferM * 100; - break; - case MAX_ALT: - default: - newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + gpsRescueConfig()->rescueAltitudeBufferM * 100); - break; - } - - //Calculate angular coefficient and offset for equation of line from 2 points needed for RESCUE_LANDING_APPROACH - lineSlope = ((float)gpsRescueConfig()->initialAltitudeM - gpsRescueConfig()->targetLandingAltitudeM) / (newDescentDistanceM - gpsRescueConfig()->targetLandingDistanceM); - lineOffsetM = gpsRescueConfig()->initialAltitudeM - lineSlope * newDescentDistanceM; - - rescueState.phase = RESCUE_ATTAIN_ALT; - FALLTHROUGH; - case RESCUE_ATTAIN_ALT: - // Get to a safe altitude at a low velocity ASAP - if (ABS(rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) < 1000) { - rescueState.phase = RESCUE_CROSSTRACK; - } - - rescueState.intent.targetGroundspeed = 500; - rescueState.intent.targetAltitudeCm = newAltitude; - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = 15; - break; - case RESCUE_CROSSTRACK: - if (rescueState.sensor.distanceToHomeM <= newDescentDistanceM) { - rescueState.phase = RESCUE_LANDING_APPROACH; - } - - // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt - // Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT - rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed; - rescueState.intent.targetAltitudeCm = newAltitude; - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 15; - rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; - break; - case RESCUE_LANDING_APPROACH: - // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase - if (rescueState.sensor.distanceToHomeM <= gpsRescueConfig()->targetLandingDistanceM && rescueState.sensor.currentAltitudeCm <= gpsRescueConfig()->targetLandingAltitudeM * 100) { + // we didn't get a home point on arming + rescueState.failure = RESCUE_NO_HOME_POINT; // abort, or 20s of do nothing, as per sanity check settings + rescueState.phase = RESCUE_DO_NOTHING; + } else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { + // Attempt to initiate inside minimum activation distance -> landing mode + rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm - rescueState.sensor.descendStepCm; rescueState.phase = RESCUE_LANDING; - } - - // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) - const int32_t newAlt = MAX((lineSlope * rescueState.sensor.distanceToHomeM + lineOffsetM) * 100, 0); - - // Start to decrease proportionally the quad's speed when the distance to home is less or equal than GPS_RESCUE_SLOWDOWN_DISTANCE_M - if (rescueState.sensor.distanceToHomeM <= GPS_RESCUE_SLOWDOWN_DISTANCE_M) { - newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / GPS_RESCUE_SLOWDOWN_DISTANCE_M; - } - - rescueState.intent.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm); - rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; - break; - case RESCUE_LANDING: - // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. - // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory - // If we are over 150% of average magnitude, just disarm since we're pretty much home - if (rescueState.sensor.currentAltitudeCm < GPS_RESCUE_ALMOST_LANDING_ALT) { - magnitudeTrigger = rescueState.sensor.accMagnitudeAvg * 1.2; + // start landing from current altitude } else { - magnitudeTrigger = rescueState.sensor.accMagnitudeAvg * 1.5; + startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm); + rescueState.phase = RESCUE_ATTAIN_ALT; } + break; - if (rescueState.sensor.accMagnitude > magnitudeTrigger) { + case RESCUE_ATTAIN_ALT: + // increase target altitude from current altitude towards the "fly home" altitude at ascent or descent rate + // sanity check will abort if altitude gain is blocked in a reasonable time + // exit once we are within one ascend/descend step of the target altitude + // while climbing, rotate; use pitch only to get velocity to home close to zero if possible + if (newGPSData) { + if (startedLow) { + if (rescueState.sensor.currentAltitudeCm > (rescueState.intent.returnAltitudeCm - rescueState.sensor.ascendStepCm)) { + rescueState.phase = RESCUE_ROTATE; + } else if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) { + rescueState.intent.targetAltitudeCm += rescueState.sensor.ascendStepCm; + // note targetAltitudeCm is dynamically updated each new GPS data in idleTasks() + } + } else { + if (rescueState.sensor.currentAltitudeCm < (rescueState.intent.returnAltitudeCm + rescueState.sensor.descendStepCm)) { + rescueState.phase = RESCUE_ROTATE; + } else if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) { + rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm; + } + } + // try to correct velocity relative to home by allowing pitch corrections with zero target velocity + rescueState.intent.targetVelocityCmS = 0; + rescueState.intent.minAnglePitchDeg = -halfAngle; // reduces forward velocity + rescueState.intent.maxAnglePitchDeg = halfAngle; + rescueState.intent.updateYaw = true; // allow yaw during the climb, since this helps gain altitude + } + rescueState.intent.maxAngleRollDeg = 0; // no roll yet + break; + + case RESCUE_ROTATE: + // we may bypass attain_alt so this must stand alone. + // complete the rotation, allowing pitch when pointing towards home + if (newGPSData) { + if (rescueState.sensor.errorAngle < 15.0f) { + rescueState.phase = RESCUE_FLY_HOME; + // include roll corrections + } else if (rescueState.sensor.errorAngle < 45.0f) { + // pitch forward + rescueState.intent.minAnglePitchDeg = -gpsRescueConfig()->angle; + rescueState.intent.maxAnglePitchDeg = gpsRescueConfig()->angle; + rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed; + } else { + rescueState.intent.minAnglePitchDeg = -halfAngle; + rescueState.intent.maxAnglePitchDeg = halfAngle; + rescueState.intent.targetVelocityCmS = 0; + } + rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; + rescueState.intent.updateYaw = true; + } + rescueState.intent.maxAngleRollDeg = 0; // no roll until yaw error is less than 15 degrees + break; + + case RESCUE_FLY_HOME: + // fly home with full control on all axes, pitching forward to gain speed + if (newGPSData) { + if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) { + rescueState.phase = RESCUE_DESCENT; + } + } + rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; + rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed; + rescueState.intent.minAnglePitchDeg = -gpsRescueConfig()->angle; + rescueState.intent.maxAnglePitchDeg = gpsRescueConfig()->angle; + rescueState.intent.updateYaw = true; + rescueState.intent.maxAngleRollDeg = gpsRescueConfig()->angle; + break; + + case RESCUE_DESCENT: + // attenuate velocity and altitude targets while updating the heading to home + // once inside the landing box, stop rotating, just descend + if (newGPSData) { + const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM; + if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->targetLandingDistanceM && rescueState.sensor.currentAltitudeCm > targetLandingAltitudeCm) { + // outside the landing box + const float proximityToHome = constrainf(rescueState.sensor.distanceToHomeM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); + const float targetAltitudeModified = (proximityToHome * (rescueState.intent.returnAltitudeCm - targetLandingAltitudeCm)) + targetLandingAltitudeCm; + // aim for top of box + float targetAltitudeWhileDescendingCm = 0.0f; + targetAltitudeWhileDescendingCm = MIN(targetAltitudeWhileDescendingCm, targetAltitudeModified); + // don't let target altitude increase. leads to landing 'short of the box' but much safer if it overshoots, won't climb again + float landingDescendStepCm = rescueState.sensor.descendStepCm * (1.0f + proximityToHome); + // this is the maximum allowed step in target altitude, a bit faster at the start, since velocity also will be faster at the start + rescueState.intent.targetAltitudeCm -= constrain((rescueState.sensor.currentAltitudeCm - targetAltitudeWhileDescendingCm), 0, landingDescendStepCm); + // reduce current altitude towards target altitude, but not by more than landingDescendStepCm + rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToHome; + // slow down as we get closer to home, allowing zero for vertical drops + } else { + // go to landing mode once inside the box, with zero groundspeed target + rescueState.phase = RESCUE_LANDING; + rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm; + // take one descent step off our target altitude now + } + } + rescueState.intent.minAnglePitchDeg = -gpsRescueConfig()->angle; // aids slowing down + rescueState.intent.maxAnglePitchDeg = gpsRescueConfig()->angle; + rescueState.intent.updateYaw = true; + rescueState.intent.maxAngleRollDeg = gpsRescueConfig()->angle; + break; + + case RESCUE_LANDING: + // let Level Mode keep the quad flat and true and just lose altitude until it hits the ground + // it will be moved crabwise by wind and momentum but we don't know how best to respond; better to do nothing at all + // Note: no iTerm on throttle during landing + if (newGPSData) { + rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm; + // take one step off target altitude every time we get new GPS data + } + // keep descending at descendStepCm until we hit the ground + // can't use altitude since the "zero altitude" value may be incorrect + + if (rescueState.sensor.accMagnitude > 2.0f) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); disarm(DISARM_REASON_GPS_RESCUE); rescueState.phase = RESCUE_COMPLETE; } - rescueState.intent.targetGroundspeed = 0; - rescueState.intent.targetAltitudeCm = 0; - rescueState.intent.crosstrack = true; - rescueState.intent.minAngleDeg = 0; - rescueState.intent.maxAngleDeg = 15; + rescueState.intent.minAnglePitchDeg = -halfAngle; + rescueState.intent.maxAnglePitchDeg = halfAngle; + rescueState.intent.targetVelocityCmS = 0.0f; + rescueState.intent.updateYaw = true; + rescueState.intent.maxAngleRollDeg = 0; + // keep current heading and attitude, just drop break; + case RESCUE_COMPLETE: rescueStop(); break; + case RESCUE_ABORT: setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); disarm(DISARM_REASON_GPS_RESCUE); rescueStop(); break; + + case RESCUE_DO_NOTHING: + break; default: break; } + DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, rescueState.intent.targetAltitudeCm); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.intent.targetAltitudeCm); + DEBUG_SET(DEBUG_RTH, 1, rescueState.phase); + performSanityChecks(); if (rescueState.phase != RESCUE_IDLE) { diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index b1b575bac9..381e788102 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -26,12 +26,12 @@ typedef struct gpsRescue_s { uint16_t initialAltitudeM; //meters uint16_t descentDistanceM; //meters uint16_t rescueGroundspeed; //centimeters per second - uint16_t throttleP, throttleI, throttleD; - uint16_t yawP; + uint8_t throttleP, throttleI, throttleD; + uint8_t yawP; uint16_t throttleMin; uint16_t throttleMax; uint16_t throttleHover; - uint16_t velP, velI, velD; + uint8_t velP, velI, velD; uint8_t minSats; uint16_t minRescueDth; //meters uint8_t sanityChecks; @@ -43,11 +43,12 @@ typedef struct gpsRescue_s { uint16_t ascendRate; uint16_t descendRate; uint16_t rescueAltitudeBufferM; //meters + uint8_t rollMix; } gpsRescueConfig_t; 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 rescueNewGpsData(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 442c47c328..d1b8ca2a88 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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_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 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]; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index a830e92f11..c663eafe1c 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -81,13 +81,13 @@ static char *gpsPacketLogChar = gpsPacketLog; // ********************** int32_t GPS_home[2]; 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 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; -#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; uint32_t GPS_packetCount = 0; @@ -1838,10 +1838,12 @@ void GPS_calculateDistanceAndDirectionToHome(void) 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); - GPS_distanceToHome = dist / 100; - GPS_directionToHome = dir / 100; + GPS_distanceToHome = dist / 100; // m/s + GPS_distanceToHomeCm = dist; // cm/sec + GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees } else { GPS_distanceToHome = 0; + GPS_distanceToHomeCm = 0; GPS_directionToHome = 0; } } @@ -1852,16 +1854,6 @@ void onGpsNewData(void) 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(); if (ARMING_FLAG(ARMED)) { GPS_calculateDistanceFlownVerticalSpeed(false); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 6047ac2590..26fb4685f7 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -142,11 +142,11 @@ extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; extern int32_t GPS_home[2]; 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 uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters 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 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 int16_t nav_takeoff_bearing; diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 6321342a39..5cd05cea83 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -1023,7 +1023,7 @@ static void osdElementGpsHomeDirection(osdElementParms_t *element) { if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { 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); } else { element->buff[0] = SYM_OVER_HOME; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index fba097c5cd..8258201dcf 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -674,6 +674,7 @@ void detectAndApplySignalLossBehaviour(void) sample = getRxfailValue(channel); // set channels to Stage 1 values immediately failsafe switch is activated } else if (!thisChannelValid) { + // everything was normal and this channel was invalid if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) { // first 300ms of Stage 1 failsafe sample = rcData[channel]; diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index be6678d15a..2dcf6430e8 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -144,7 +144,7 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) 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, GPS_directionToHome); + sbufWriteU16(dst, GPS_directionToHome / 10); uint8_t gpsFlags = 0; if (STATE(GPS_FIX)) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 88c7c72fdf..328de51523 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -244,7 +244,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_H = hottGpsAltitude >> 8; - hottGPSMessage->home_direction = GPS_directionToHome; + hottGPSMessage->home_direction = GPS_directionToHome / 10; } #endif diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 749ee1d836..8b9b609e97 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -365,7 +365,7 @@ int32_t getSensorValue(uint8_t sensor) break; case EX_GPS_DIRECTION_TO_HOME: - return GPS_directionToHome; + return GPS_directionToHome / 10; break; case EX_GPS_HEADING: diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index df2d4bcd32..9ac5dfdb93 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -72,7 +72,7 @@ extern "C" { bool cmsInMenu = false; float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3]; rxRuntimeState_t rxRuntimeState = {}; - uint16_t GPS_distanceToHome = 0; + uint32_t GPS_distanceToHomeCm = 0; int16_t GPS_directionToHome = 0; acc_t acc = {}; bool mockIsUpright = false; @@ -1059,6 +1059,8 @@ extern "C" { void failsafeStartMonitoring(void) {} void failsafeUpdateState(void) {} bool failsafeIsActive(void) { return false; } + bool failsafeIsReceivingRxData(void) { return false; } + bool rxAreFlightChannelsValid(void) { return false; } void pidResetIterm(void) {} void updateAdjustmentStates(void) {} void processRcAdjustments(controlRateConfig_t *) {} @@ -1109,4 +1111,5 @@ extern "C" { bool isMotorProtocolEnabled(void) { return true; } void pinioBoxTaskControl(void) {} void schedulerSetNextStateTime(timeDelta_t) {} + float pt1FilterGain(float, float) {return 0.5f;} } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 0c20e1f938..a03e754506 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -513,46 +513,17 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage1OrStage2Drop) // deactivate the failsafe switch 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(isArmingDisabled()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); 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 + // by next evaluation we should be out of failsafe sysTickUptime ++; + // receivingRxData is immediately true because signal exists + failsafeOnValidDataReceived(); - // when + // when failsafeUpdateState(); // we should now have exited failsafe @@ -647,34 +618,10 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land) EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); 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 sysTickUptime ++; + failsafeOnValidDataReceived(); // when failsafeUpdateState(); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index cb2482247b..2053988693 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -626,6 +626,7 @@ void dashboardEnablePageCycling() {} bool failsafeIsActive() { return false; } bool rxIsReceivingSignal() { return true; } +bool failsafeIsReceivingRxData() { return true; } uint8_t getCurrentControlRateProfileIndex(void) { return 0; diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index d7d1c2f277..f09f2d62db 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -152,6 +152,8 @@ extern "C" { void failsafeStartMonitoring(void) {} void failsafeUpdateState(void) {} bool failsafeIsActive(void) { return false; } + bool rxAreFlightChannelsValid(void) { return false; } + bool failsafeIsReceivingRxData(void) { return false; } void pidResetIterm(void) {} void updateAdjustmentStates(void) {} void processRcAdjustments(controlRateConfig_t *) {}