From 254da8f4605ef2b7b75e7054b1c9f907e1941c20 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Wed, 4 Sep 2024 20:29:03 +1000 Subject: [PATCH] Altitude hold for 4.6 (#13816) --- mk/source.mk | 2 + src/main/blackbox/blackbox.c | 37 ++- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/cli/settings.c | 29 +- src/main/cms/cms_menu_failsafe.c | 8 +- src/main/cms/cms_menu_gps_rescue.c | 17 +- src/main/fc/core.c | 34 ++- src/main/fc/init.c | 5 + src/main/fc/parameter_names.h | 29 +- src/main/fc/rc.c | 4 +- src/main/fc/rc_controls.c | 3 +- src/main/fc/rc_modes.c | 2 +- src/main/fc/rc_modes.h | 1 + src/main/fc/runtime_config.h | 3 +- src/main/fc/tasks.c | 9 + src/main/flight/alt_hold.c | 287 ++++++++++++++++++++ src/main/flight/alt_hold.h | 41 +++ src/main/flight/failsafe.c | 4 +- src/main/flight/failsafe.h | 2 +- src/main/flight/gps_rescue.c | 14 +- src/main/flight/imu.c | 4 +- src/main/flight/mixer.c | 10 +- src/main/flight/pid.c | 55 ++-- src/main/flight/position.c | 24 +- src/main/flight/position.h | 4 + src/main/msp/msp.c | 8 +- src/main/msp/msp_box.c | 5 +- src/main/osd/osd_elements.c | 4 +- src/main/pg/alt_hold.c | 42 +++ src/main/pg/alt_hold.h | 37 +++ src/main/pg/gps_rescue.c | 6 +- src/main/pg/gps_rescue.h | 2 - src/main/pg/pg_ids.h | 1 + src/main/scheduler/scheduler.h | 3 + src/main/target/common_pre.h | 1 + src/main/telemetry/crsf.c | 2 + src/main/telemetry/ibus_shared.c | 4 +- src/main/telemetry/ltm.c | 2 + src/main/telemetry/mavlink.c | 2 +- src/main/telemetry/smartport.c | 2 +- src/test/Makefile | 8 + src/test/unit/althold_unittest.cc | 134 +++++++++ src/test/unit/arming_prevention_unittest.cc | 3 +- src/test/unit/flight_failsafe_unittest.cc | 8 +- src/test/unit/pid_unittest.cc | 9 +- 46 files changed, 805 insertions(+), 108 deletions(-) create mode 100644 src/main/flight/alt_hold.c create mode 100644 src/main/flight/alt_hold.h create mode 100644 src/main/pg/alt_hold.c create mode 100644 src/main/pg/alt_hold.h create mode 100644 src/test/unit/althold_unittest.cc diff --git a/mk/source.mk b/mk/source.mk index 38d1063b0b..c670d5892e 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -1,5 +1,6 @@ PG_SRC = \ pg/adc.c \ + pg/alt_hold.c \ pg/beeper.c \ pg/beeper_dev.c \ pg/board.c \ @@ -158,6 +159,7 @@ COMMON_SRC = \ flight/gps_rescue.c \ fc/gps_lap_timer.c \ flight/dyn_notch_filter.c \ + flight/alt_hold.c \ flight/imu.c \ flight/mixer.c \ flight/mixer_init.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1f004489cf..90b768f95f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -62,13 +62,14 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "flight/alt_hold.h" #include "flight/failsafe.h" +#include "flight/gps_rescue.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "flight/position.h" #include "flight/rpm_filter.h" #include "flight/servos.h" -#include "flight/gps_rescue.h" -#include "flight/position.h" #include "io/beeper.h" #include "io/gps.h" @@ -93,7 +94,7 @@ #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_NONE #endif -PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 4); PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, .fields_disabled_mask = 0, // default log all fields @@ -1581,10 +1582,12 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_BARO BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware); #endif - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_SOURCE, "%d", positionConfig()->altitude_source); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_SOURCE, "%d", positionConfig()->altitude_source); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", positionConfig()->hover_throttle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", positionConfig()->landing_altitude_m); #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware); @@ -1675,12 +1678,10 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold) 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_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) @@ -1694,16 +1695,24 @@ static bool blackboxWriteSysinfo(void) 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) - #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag) -#endif -#endif +#endif // USE_MAG +#endif // USE_GPS_RESCUE +#endif // USE_GPS + +#ifdef USE_ALT_HOLD_MODE + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_P, "%d", altholdConfig()->alt_hold_pid_p); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_I, "%d", altholdConfig()->alt_hold_pid_i); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_D, "%d", altholdConfig()->alt_hold_pid_d); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_MIN, "%d", altholdConfig()->alt_hold_throttle_min); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_MAX, "%d", altholdConfig()->alt_hold_throttle_max); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, "%d", altholdConfig()->alt_hold_target_adjust_rate); #endif #ifdef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS, "%d", currentPidProfile->tpa_delay_ms); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR0, "%d", currentPidProfile->tpa_gravity_thr0); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS, "%d", currentPidProfile->tpa_delay_ms); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR0, "%d", currentPidProfile->tpa_gravity_thr0); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR100, "%d", currentPidProfile->tpa_gravity_thr100); #endif diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 06e96582bb..26628a285a 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -121,4 +121,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "S_TERM", "SPA", "TASK", + "ALTHOLD", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 173f9404bf..ccaad5c52e 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -123,6 +123,7 @@ typedef enum { DEBUG_S_TERM, DEBUG_SPA, DEBUG_TASK, + DEBUG_ALTHOLD, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e507868441..6a334f675a 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -62,6 +62,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/position.h" +#include "flight/alt_hold.h" #include "flight/rpm_filter.h" #include "flight/servos.h" @@ -887,7 +888,7 @@ const clivalue_t valueTable[] = { // PG_FAILSAFE_CONFIG { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { PERIOD_RXDATA_RECOVERY / MILLIS_PER_TENTH_SECOND, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_delay) }, - { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_off_delay) }, + { "failsafe_landing_time", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_landing_time) }, { "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle) }, { "failsafe_switch_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE_SWITCH_MODE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_switch_mode) }, { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 300 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle_low_delay) }, @@ -1060,12 +1061,10 @@ const clivalue_t valueTable[] = { { 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_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) }, - { PARAM_NAME_GPS_RESCUE_LANDING_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) }, { PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, disarmThreshold) }, { 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_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) }, @@ -1097,6 +1096,16 @@ const clivalue_t valueTable[] = { { PARAM_NAME_YAW_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, { "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, +#ifdef USE_ALT_HOLD_MODE + { PARAM_NAME_ALT_HOLD_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_pid_p) }, + { PARAM_NAME_ALT_HOLD_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_pid_i) }, + { PARAM_NAME_ALT_HOLD_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_pid_d) }, + + { PARAM_NAME_ALT_HOLD_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_throttle_min) }, + { PARAM_NAME_ALT_HOLD_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_throttle_max) }, + { PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_target_adjust_rate) }, +#endif + // PG_PID_CONFIG { PARAM_NAME_PID_PROCESS_DENOM, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) }, #ifdef USE_RUNAWAY_TAKEOFF @@ -1811,16 +1820,18 @@ const clivalue_t valueTable[] = { #endif // USE_PERSISTENT_STATS - { "craft_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, craftName) }, + { "craft_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, craftName) }, #ifdef USE_OSD - { "pilot_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, pilotName) }, + { "pilot_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, pilotName) }, #endif // PG_POSITION - { "altitude_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altitude_source) }, - { "altitude_prefer_baro", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altitude_prefer_baro) }, - { "altitude_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_lpf) }, - { "altitude_d_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) }, + { PARAM_NAME_ALTITUDE_SOURCE, VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altitude_source) }, + { PARAM_NAME_ALTITUDE_PREFER_BARO, VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altitude_prefer_baro) }, + { PARAM_NAME_ALTITUDE_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_lpf) }, + { PARAM_NAME_ALTITUDE_D_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) }, + { PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_POSITION, offsetof(positionConfig_t, hover_throttle) }, + { PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION, offsetof(positionConfig_t, landing_altitude_m) }, // PG_MODE_ACTIVATION_CONFIG #if defined(USE_CUSTOM_BOX_NAMES) diff --git a/src/main/cms/cms_menu_failsafe.c b/src/main/cms/cms_menu_failsafe.c index e8bb4f1838..16f712b8bb 100644 --- a/src/main/cms/cms_menu_failsafe.c +++ b/src/main/cms/cms_menu_failsafe.c @@ -45,7 +45,7 @@ uint8_t failsafeConfig_failsafe_procedure; uint8_t failsafeConfig_failsafe_delay; -uint8_t failsafeConfig_failsafe_off_delay; +uint8_t failsafeConfig_failsafe_landing_time; uint16_t failsafeConfig_failsafe_throttle; static const void *cmsx_Failsafe_onEnter(displayPort_t *pDisp) @@ -54,7 +54,7 @@ static const void *cmsx_Failsafe_onEnter(displayPort_t *pDisp) failsafeConfig_failsafe_procedure = failsafeConfig()->failsafe_procedure; failsafeConfig_failsafe_delay = failsafeConfig()->failsafe_delay; - failsafeConfig_failsafe_off_delay = failsafeConfig()->failsafe_off_delay; + failsafeConfig_failsafe_landing_time = failsafeConfig()->failsafe_landing_time; failsafeConfig_failsafe_throttle = failsafeConfig()->failsafe_throttle; return NULL; @@ -67,7 +67,7 @@ static const void *cmsx_Failsafe_onExit(displayPort_t *pDisp, const OSD_Entry *s failsafeConfigMutable()->failsafe_procedure = failsafeConfig_failsafe_procedure; failsafeConfigMutable()->failsafe_delay = failsafeConfig_failsafe_delay; - failsafeConfigMutable()->failsafe_off_delay = failsafeConfig_failsafe_off_delay; + failsafeConfigMutable()->failsafe_landing_time = failsafeConfig_failsafe_landing_time; failsafeConfigMutable()->failsafe_throttle = failsafeConfig_failsafe_throttle; return NULL; @@ -79,7 +79,7 @@ static const OSD_Entry cmsx_menuFailsafeEntries[] = { "PROCEDURE", OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &failsafeConfig_failsafe_procedure, FAILSAFE_PROCEDURE_COUNT - 1, failsafeProcedureNames } }, { "GUARD TIME", OME_FLOAT | REBOOT_REQUIRED, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_delay, PERIOD_RXDATA_RECOVERY / MILLIS_PER_TENTH_SECOND, 200, 1, 100 } }, - { "STAGE 2 DELAY", OME_FLOAT | REBOOT_REQUIRED, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_off_delay, 0, 200, 1, 100 } }, + { "LANDING_TIME", OME_FLOAT | REBOOT_REQUIRED, NULL, &(OSD_FLOAT_t) { &failsafeConfig_failsafe_landing_time, 0, 200, 1, 100 } }, { "STAGE 2 THROTTLE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t) { &failsafeConfig_failsafe_throttle, PWM_PULSE_MIN, PWM_PULSE_MAX, 1 } }, #ifdef USE_CMS_GPS_RESCUE_MENU { "GPS RESCUE", OME_Submenu, cmsMenuChange, &cmsx_menuGpsRescue}, diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c index d580cbb6ef..5be93f84a4 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/src/main/cms/cms_menu_gps_rescue.c @@ -38,6 +38,7 @@ #include "config/config.h" #include "flight/gps_rescue.h" +#include "flight/position.h" static uint16_t gpsRescueConfig_minStartDistM; //meters static uint8_t gpsRescueConfig_altitudeMode; @@ -50,11 +51,11 @@ static uint8_t gpsRescueConfig_angle; //degrees static uint16_t gpsRescueConfig_descentDistanceM; //meters static uint16_t gpsRescueConfig_descendRate; -static uint8_t gpsRescueConfig_targetLandingAltitudeM; +static uint8_t positionConfig_landingAltitudeM; static uint16_t gpsRescueConfig_throttleMin; static uint16_t gpsRescueConfig_throttleMax; -static uint16_t gpsRescueConfig_throttleHover; +static uint16_t positionConfig_hoverThrottle; static uint8_t gpsRescueConfig_minSats; static uint8_t gpsRescueConfig_allowArmingWithoutFix; @@ -154,11 +155,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM; gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate; - gpsRescueConfig_targetLandingAltitudeM = gpsRescueConfig()->targetLandingAltitudeM; + positionConfig_landingAltitudeM = positionConfig()->landing_altitude_m; gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin; gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax; - gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover; + positionConfig_hoverThrottle = positionConfig()->hover_throttle; gpsRescueConfig_minSats = gpsRescueConfig()->minSats; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; @@ -182,11 +183,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM; gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate; - gpsRescueConfigMutable()->targetLandingAltitudeM = gpsRescueConfig_targetLandingAltitudeM; + positionConfigMutable()->landing_altitude_m = positionConfig_landingAltitudeM; gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin; gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax; - gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover; + positionConfigMutable()->hover_throttle = positionConfig_hoverThrottle; gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; @@ -209,11 +210,11 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = { "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } }, { "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } }, - { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 15, 1 } }, + { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionConfig_landingAltitudeM, 1, 15, 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 } }, + { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &positionConfig_hoverThrottle, 1100, 1700, 1 } }, { "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, diff --git a/src/main/fc/core.c b/src/main/fc/core.c index a050936ac6..f9eb1e37de 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -61,6 +61,7 @@ #include "flight/failsafe.h" #include "flight/gps_rescue.h" +#include "flight/alt_hold.h" #if defined(USE_DYN_NOTCH_FILTER) #include "flight/dyn_notch_filter.h" @@ -232,6 +233,7 @@ static bool accNeedsCalibration(void) // Check for any configured modes that use the ACC if (isModeActivationConditionPresent(BOXANGLE) || isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXALTHOLD) || isModeActivationConditionPresent(BOXGPSRESCUE) || isModeActivationConditionPresent(BOXCAMSTAB) || isModeActivationConditionPresent(BOXCALIB) || @@ -968,7 +970,12 @@ void processRxModes(timeUs_t currentTimeUs) } bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { + if ((IS_RC_MODE_ACTIVE(BOXANGLE) + || failsafeIsActive() +#ifdef USE_ALT_HOLD_MODE + || FLIGHT_MODE(ALT_HOLD_MODE) +#endif + ) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; @@ -979,10 +986,29 @@ void processRxModes(timeUs_t currentTimeUs) DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } +#ifdef USE_ALT_HOLD_MODE + // only if armed + if (ARMING_FLAG(ARMED) + // and either the alt_hold switch is activated, or are in failsafe + && (IS_RC_MODE_ACTIVE(BOXALTHOLD) || failsafeIsActive()) + // but not in GPS_RESCUE_MODE, ie if failsafe is active, must be in Landing Mode + && !FLIGHT_MODE(GPS_RESCUE_MODE) + // and we have Acc for self-levelling + && sensors(SENSOR_ACC) + // and we have altitude data + && isAltitudeAvailable() + // and we have already taken off (to prevent activation on the ground), then enable althold + && isAirmodeActivated()) { + if (!FLIGHT_MODE(ALT_HOLD_MODE)) { + ENABLE_FLIGHT_MODE(ALT_HOLD_MODE); + } + } else { + DISABLE_FLIGHT_MODE(ALT_HOLD_MODE); + } +#endif + if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { - DISABLE_FLIGHT_MODE(ANGLE_MODE); - if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } @@ -1000,7 +1026,7 @@ void processRxModes(timeUs_t currentTimeUs) } #endif - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE)) { LED1_ON; // increase frequency of attitude task to reduce drift when in angle or horizon mode rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(acc.sampleRateHz / (float)imuConfig()->imu_process_denom)); diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 9cbee07e90..32ebaad959 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -94,6 +94,7 @@ #include "fc/stats.h" #include "fc/tasks.h" +#include "flight/alt_hold.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -999,6 +1000,10 @@ void init(void) spiInitBusDMA(); #endif +#ifdef USE_ALT_HOLD_MODE + altHoldInit(); +#endif + debugInit(); unusedPinsInit(); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 8d57111743..5d35e2225b 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -148,10 +148,12 @@ #define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz" #define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz" #define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz" -#define PARAM_NAME_POSITION_ALTITUDE_SOURCE "altitude_source" -#define PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO "altitude_prefer_baro" -#define PARAM_NAME_POSITION_ALTITUDE_LPF "altitude_lpf" -#define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf" +#define PARAM_NAME_ALTITUDE_SOURCE "altitude_source" +#define PARAM_NAME_ALTITUDE_PREFER_BARO "altitude_prefer_baro" +#define PARAM_NAME_ALTITUDE_LPF "altitude_lpf" +#define PARAM_NAME_ALTITUDE_D_LPF "altitude_d_lpf" +#define PARAM_NAME_HOVER_THROTTLE "hover_throttle" +#define PARAM_NAME_LANDING_ALTITUDE "landing_altitude_m" #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward" #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" #define PARAM_NAME_ANGLE_LIMIT "angle_limit" @@ -197,12 +199,10 @@ #define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist" #define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate" -#define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt" #define PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD "gps_rescue_disarm_threshold" #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_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" @@ -218,8 +218,9 @@ #ifdef USE_MAG #define PARAM_NAME_GPS_RESCUE_USE_MAG "gps_rescue_use_mag" -#endif -#endif +#endif // USE_MAG + +#endif // USE_GPS_RESCUE #ifdef USE_GPS_LAP_TIMER #define PARAM_NAME_GPS_LAP_TIMER_GATE_LAT "gps_lap_timer_gate_lat" @@ -227,7 +228,17 @@ #define PARAM_NAME_GPS_LAP_TIMER_MIN_LAP_TIME "gps_lap_timer_min_lap_time_s" #define PARAM_NAME_GPS_LAP_TIMER_GATE_TOLERANCE "gps_lap_timer_gate_tolerance_m" #endif // USE_GPS_LAP_TIMER -#endif + +#endif // USE_GPS + +#ifdef USE_ALT_HOLD_MODE +#define PARAM_NAME_ALT_HOLD_P "alt_hold_p" +#define PARAM_NAME_ALT_HOLD_I "alt_hold_i" +#define PARAM_NAME_ALT_HOLD_D "alt_hold_d" +#define PARAM_NAME_ALT_HOLD_THROTTLE_MIN "alt_hold_throttle_min" +#define PARAM_NAME_ALT_HOLD_THROTTLE_MAX "alt_hold_throttle_max" +#define PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE "alt_hold_target_adjust_rate" +#endif // USE_ALT_HOLD_MODE #define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp" #define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 7e704ace85..8f9d37331e 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -785,7 +785,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void) rcCommandBuff.X = rcCommand[ROLL]; rcCommandBuff.Y = rcCommand[PITCH]; - if ((!FLIGHT_MODE(ANGLE_MODE) && (!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) { + if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) { rcCommandBuff.Z = rcCommand[YAW]; } else { rcCommandBuff.Z = 0; @@ -793,7 +793,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void) imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff); rcCommand[ROLL] = rcCommandBuff.X; rcCommand[PITCH] = rcCommandBuff.Y; - if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) { + if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) { rcCommand[YAW] = rcCommandBuff.Z; } } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 4383c1cb1e..00246035be 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -303,8 +303,7 @@ void processRcStickPositions(void) } #endif - - if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) { + if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE)) { // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims rollAndPitchTrims_t accelerometerTrimsDelta; memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta)); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index ef3b5d2640..94ed8507d7 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -56,7 +56,7 @@ static uint8_t activeMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT]; static int activeLinkedMacCount = 0; static uint8_t activeLinkedMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT]; -PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 2); +PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 3); #if defined(USE_CUSTOM_BOX_NAMES) PG_REGISTER_WITH_RESET_TEMPLATE(modeActivationConfig_t, modeActivationConfig, PG_MODE_ACTIVATION_CONFIG, 0); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index f50c59b373..e6282fcee4 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -33,6 +33,7 @@ typedef enum { BOXANGLE, BOXHORIZON, BOXMAG, + BOXALTHOLD, BOXHEADFREE, BOXPASSTHRU, BOXFAILSAFE, diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index d5e8ba4727..271b317d20 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -81,7 +81,7 @@ typedef enum { ANGLE_MODE = (1 << 0), HORIZON_MODE = (1 << 1), MAG_MODE = (1 << 2), -// BARO_MODE = (1 << 3), + ALT_HOLD_MODE = (1 << 3), // GPS_HOME_MODE = (1 << 4), // GPS_HOLD_MODE = (1 << 5), HEADFREE_MODE = (1 << 6), @@ -104,6 +104,7 @@ extern uint16_t flightModeFlags; [BOXANGLE] = LOG2(ANGLE_MODE), \ [BOXHORIZON] = LOG2(HORIZON_MODE), \ [BOXMAG] = LOG2(MAG_MODE), \ + [BOXALTHOLD] = LOG2(ALT_HOLD_MODE), \ [BOXHEADFREE] = LOG2(HEADFREE_MODE), \ [BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \ [BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \ diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 096a332ba7..351669f502 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -58,6 +58,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/position.h" +#include "flight/alt_hold.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -382,6 +383,10 @@ task_attribute_t task_attributes[TASK_COUNT] = { [TASK_GPS_RESCUE] = DEFINE_TASK("GPS_RESCUE", NULL, NULL, taskGpsRescue, TASK_PERIOD_HZ(TASK_GPS_RESCUE_RATE_HZ), TASK_PRIORITY_MEDIUM), #endif +#ifdef USE_ALT_HOLD_MODE + [TASK_ALTHOLD] = DEFINE_TASK("ALTHOLD", NULL, NULL, updateAltHoldState, TASK_PERIOD_HZ(ALTHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW), +#endif + #ifdef USE_MAG [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ), TASK_PRIORITY_LOW), #endif @@ -537,6 +542,10 @@ void tasksInit(void) setTaskEnabled(TASK_GPS_RESCUE, featureIsEnabled(FEATURE_GPS)); #endif +#ifdef USE_ALT_HOLD_MODE + setTaskEnabled(TASK_ALTHOLD, true); +#endif + #ifdef USE_MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #endif diff --git a/src/main/flight/alt_hold.c b/src/main/flight/alt_hold.c new file mode 100644 index 0000000000..ab67b0afd2 --- /dev/null +++ b/src/main/flight/alt_hold.c @@ -0,0 +1,287 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include "platform.h" + +#ifdef USE_ALT_HOLD_MODE + +#include "math.h" +#include "build/debug.h" + +#include "config/config.h" +#include "fc/runtime_config.h" +#include "fc/rc.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/position.h" +#include "sensors/acceleration.h" +#include "rx/rx.h" + +#include "alt_hold.h" + +typedef struct { + float kp; + float ki; + float kd; + float kf; + float previousAltitude; + float integral; +} simplePid_t; + +simplePid_t simplePid; + +altHoldState_t altHoldState; + +#define ALT_HOLD_PID_P_SCALE 0.01f +#define ALT_HOLD_PID_I_SCALE 0.003f +#define ALT_HOLD_PID_D_SCALE 0.01f + +static pt2Filter_t altHoldDeltaLpf; +static const float taskIntervalSeconds = 1.0f / ALTHOLD_TASK_RATE_HZ; // i.e. 0.01 s + +float altitudePidCalculate(void) +{ + // * introductory notes * + // this is a simple PID controller with heuristic D boost and iTerm relax + // the basic parameters provide good control when initiated in stable situations + + // tuning: + // -reduce P I and D by 1/3 or until it doesn't oscillate but has sloppy / slow control + // increase P until there is definite oscillation, then back off until barely noticeable + // increase D until there is definite oscillation (will be faster than P), then back off until barely noticeable + // try to add a little more P, then try to add a little more D, but not to oscillation + // iTerm isn't very important if hover throttle is set carefully and sag compensation is used + + // The altitude D lowpass filter is very important. + // The only way to get enough D is to filter the oscillations out. + // More D filtering is needed with Baro than with GPS, since GPS is smoother and slower. + + // A major problem is the lag time for motors to arrest pre-existing drops or climbs, + // compounded by the lag time from filtering. + // If the quad is dropping fast, the motors have to be high for a long time to arrest the drop + // this is very difficult for a 'simple' PID controller; + // if the PIDs are high enough to arrest a fast drop, they will oscillate under normal conditions + // Hence we: + // - Enhance D when the absolute velocity is high, ie when we need to strongly oppose a fast drop, + // even though it may cause throttle oscillations while dropping quickly - the average D is what we need + // - Prevent excessive iTerm growth when error is impossibly large for iTerm to resolve + + const float altErrorCm = altHoldState.targetAltitudeCm - altHoldState.measuredAltitudeCm; + + // P + const float pOut = simplePid.kp * altErrorCm; + + // I + // input limit iTerm so that it doesn't grow fast with large errors + // very important at the start if there are massive initial errors to prevent iTerm windup + + // no iTerm change for error greater than 2m, otherwise it winds up badly + const float itermNormalRange = 200.0f; // 2m + const float itermRelax = (fabsf(altErrorCm) < itermNormalRange) ? 1.0f : 0.0f; + simplePid.integral += altErrorCm * taskIntervalSeconds * simplePid.ki * itermRelax; + // arbitrary limit on iTerm, same as for gps_rescue, +/-20% of full throttle range + // ** might not be needed with input limiting ** + simplePid.integral = constrainf(simplePid.integral, -200.0f, 200.0f); + const float iOut = simplePid.integral; + + // D + // boost D by 'increasing apparent velocity' when vertical velocity exceeds 5 m/s ( D of 75 on defaults) + // the velocity trigger is arbitrary at this point + // usually we don't see fast ascend/descend rates if the altitude hold starts under stable conditions + // this is important primarily to arrest pre-existing fast drops or climbs at the start; + + float vel = altHoldState.smoothedVerticalVelocity; + const float kinkPoint = 500.0f; // velocity at which D should start to increase + const float kinkPointAdjustment = kinkPoint * 2.0f; // Precompute constant + const float sign = (vel > 0) ? 1.0f : -1.0f; + if (fabsf(vel) > kinkPoint) { + vel = vel * 3.0f - sign * kinkPointAdjustment; + } + + const float dOut = simplePid.kd * vel; + + // F + // if error is used, we get a 'free kick' in derivative from changes in the target value + // but this is delayed by the smoothing, leading to lag and overshoot. + // calculating feedforward separately avoids the filter lag. + // Use user's D gain for the feedforward gain factor, works OK with a scaling factor of 0.01 + // A commanded drop at 100cm/s will return feedforward of the user's D value. or 15 on defaults + const float fOut = simplePid.kf * altHoldState.targetAltitudeAdjustRate; + + const float output = pOut + iOut + dOut + fOut; + DEBUG_SET(DEBUG_ALTHOLD, 4, lrintf(pOut)); + DEBUG_SET(DEBUG_ALTHOLD, 5, lrintf(iOut)); + DEBUG_SET(DEBUG_ALTHOLD, 6, lrintf(dOut)); + DEBUG_SET(DEBUG_ALTHOLD, 7, lrintf(fOut)); + + return output; // motor units, eg 100 means 10% of available throttle +} + +void simplePidInit(float kp, float ki, float kd, float kf) +{ + simplePid.kp = kp; + simplePid.ki = ki; + simplePid.kd = kd; + simplePid.kf = kf; + simplePid.previousAltitude = 0.0f; + simplePid.integral = 0.0f; +} + +void altHoldReset(void) +{ + altHoldState.targetAltitudeCm = altHoldState.measuredAltitudeCm; + simplePid.integral = 0.0f; + altHoldState.targetAltitudeAdjustRate = 0.0f; +} + +void altHoldInit(void) +{ + simplePidInit( + ALT_HOLD_PID_P_SCALE * altholdConfig()->alt_hold_pid_p, + ALT_HOLD_PID_I_SCALE * altholdConfig()->alt_hold_pid_i, + ALT_HOLD_PID_D_SCALE * altholdConfig()->alt_hold_pid_d, + 0.01f * altholdConfig()->alt_hold_pid_d); // use D gain for feedforward with simple scaling + // the multipliers are base scale factors + // iTerm is relatively weak, intended to be slow moving to adjust baseline errors + // the Hover value is important otherwise takes time for iTerm to correct + // High P will wobble readily + // with these scalers, the same numbers as for GPS Rescue work OK for altHold + // the goal is to share these gain factors, if practical for all altitude controllers + + //setup altitude D filter + const float cutoffHz = 0.01f * positionConfig()->altitude_d_lpf; // default 1Hz, time constant about 160ms + const float gain = pt2FilterGain(cutoffHz, taskIntervalSeconds); + pt2FilterInit(&altHoldDeltaLpf, gain); + + altHoldState.hover = positionConfig()->hover_throttle - PWM_RANGE_MIN; + altHoldState.isAltHoldActive = false; + altHoldReset(); +} + +void altHoldProcessTransitions(void) { + + if (FLIGHT_MODE(ALT_HOLD_MODE)) { + if (!altHoldState.isAltHoldActive) { + altHoldReset(); + altHoldState.isAltHoldActive = true; + } + } else { + altHoldState.isAltHoldActive = false; + } + + // ** the transition out of alt hold (exiting altHold) may be rough. Some notes... ** + // The original PR had a gradual transition from hold throttle to pilot control throttle + // using !(altHoldRequested && altHoldState.isAltHoldActive) to run an exit function + // a cross-fade factor was sent to mixer.c based on time since the flight mode request was terminated + // it was removed primarily to simplify this PR + + // hence in this PR's the user's throttle needs to be close to the hover throttle value on exiting altHold + // its not so bad because the 'target adjustment' by throttle requires that + // user throttle must be not more than half way out from hover for a stable hold +} + +void altHoldUpdateTargetAltitude(void) +{ + // The user can raise or lower the target altitude with throttle up; there is a big deadband. + // Max rate for climb/descend is 1m/s by default (up to 2.5 is allowed but overshoots a fair bit) + // If set to zero, the throttle has no effect. + + // Some people may not like throttle being able to change the target altitude, because: + // - with throttle adjustment, hitting the switch won't always hold altitude if throttle is bumped + // - eg if the throttle is bumped low accidentally, quad will start descending. + // On the plus side: + // - the pilot has control nice control over altitude, and the deadband is wide + // - Slow controlled descents are possible, eg for landing + // - fine-tuning height is possible, eg if there is slow sensor drift + // - to keep the craft stable, throttle must be in the deadband, making exits smoother + + const float rcThrottle = rcCommand[THROTTLE]; + + const float lowThreshold = 0.5f * (positionConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300 + const float highThreshold = 0.5f * (positionConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300 + + float throttleAdjustmentFactor = 0.0f; + if (rcThrottle < lowThreshold) { + throttleAdjustmentFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); + } else if (rcThrottle > highThreshold) { + throttleAdjustmentFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f); + } + + // if failsafe is active, and we get here, we are in failsafe landing mode + if (failsafeIsActive()) { + // descend at up to 10 times faster when high + // default landing time is now 60s; need to get the quad down in this time from reasonable height + // need a rapid descent if high to avoid that timeout, and must slow down closer to ground + // this code doubles descent rate at 20m, to max 10x (10m/s on defaults) at 200m + // user should be able to descend within 60s from around 150m high without disarming, on defaults + // the deceleration may be a bit rocky if it starts very high up + // constant (set) deceleration target in the last 2m + throttleAdjustmentFactor = -(0.9f + constrainf(altHoldState.measuredAltitudeCm * (1.0f / 2000.f), 0.1f, 9.0f)); + } + + altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate; + // if taskRate is 100Hz, default adjustRate of 100 adds/subtracts 1m every second, or 1cm per task run, at full stick position + altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate * taskIntervalSeconds; +} + +void altHoldUpdate(void) +{ + // check if the user has changed the target altitude using sticks + if (altholdConfig()->alt_hold_target_adjust_rate) { + altHoldUpdateTargetAltitude(); + } + + // use PIDs to return the throttle adjustment value, add it to the hover value, and constrain + const float throttleAdjustment = altitudePidCalculate(); + + const float tiltMultiplier = 2.0f - fmaxf(getCosTiltAngle(), 0.5f); + // 1 = flat, 1.24 at 40 degrees, max 1.5 around 60 degrees, the default limit of Angle Mode + // 2 - cos(x) is between 1/cos(x) and 1/sqrt(cos(x)) in this range + const float newThrottle = PWM_RANGE_MIN + (altHoldState.hover + throttleAdjustment) * tiltMultiplier; + altHoldState.throttleOut = constrainf(newThrottle, altholdConfig()->alt_hold_throttle_min, altholdConfig()->alt_hold_throttle_max); + + DEBUG_SET(DEBUG_ALTHOLD, 0, lrintf(altHoldState.targetAltitudeCm)); + DEBUG_SET(DEBUG_ALTHOLD, 2, lrintf(throttleAdjustment)); +} + +void updateAltHoldState(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + + // things that always happen + // calculate smoothed altitude Delta always, for effective value on 1st pass + altHoldState.measuredAltitudeCm = getAltitude(); + float derivative = (simplePid.previousAltitude - altHoldState.measuredAltitudeCm) / taskIntervalSeconds; // cm/s + simplePid.previousAltitude = altHoldState.measuredAltitudeCm; + + // smooth the derivative here to always have a current value, without delay due to filter lag + // this way we immediately have useful D on initialising the hold + altHoldState.smoothedVerticalVelocity = pt2FilterApply(&altHoldDeltaLpf, derivative); + + DEBUG_SET(DEBUG_ALTHOLD, 1, lrintf(altHoldState.measuredAltitudeCm)); + + altHoldProcessTransitions(); + + if (altHoldState.isAltHoldActive) { + altHoldUpdate(); + } +} + +float altHoldGetThrottle(void) { + return scaleRangef(altHoldState.throttleOut, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f); +} + +#endif diff --git a/src/main/flight/alt_hold.h b/src/main/flight/alt_hold.h new file mode 100644 index 0000000000..c7ccf32265 --- /dev/null +++ b/src/main/flight/alt_hold.h @@ -0,0 +1,41 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#include "pg/alt_hold.h" + +#ifdef USE_ALT_HOLD_MODE +#include "common/time.h" + +#define ALTHOLD_TASK_RATE_HZ 100 // hz + +typedef struct { + bool isAltHoldActive; + float targetAltitudeCm; + float targetAltitudeAdjustRate; + float measuredAltitudeCm; + float throttleOut; + float hover; + float smoothedVerticalVelocity; +} altHoldState_t; + +void altHoldInit(void); +void updateAltHoldState(timeUs_t currentTimeUs); +float altHoldGetThrottle(void); + +#endif diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2a83a7910b..cc2371b445 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -72,7 +72,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_throttle = 1000, // default throttle off. .failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition .failsafe_delay = 15, // 1.5 sec stage 1 period, can regain control on signal recovery, at idle in drop mode - .failsafe_off_delay = 10, // 1 sec in landing phase, if enabled + .failsafe_landing_time = 60, // 60 sec allowed in landing phase, if enabled, before disarm .failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // default failsafe switch action is identical to rc link loss .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT, // default full failsafe procedure is 0: auto-landing .failsafe_recovery_delay = DEFAULT_FAILSAFE_RECOVERY_DELAY, @@ -319,7 +319,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) // Enter Stage 2 with settings for landing mode ENABLE_FLIGHT_MODE(FAILSAFE_MODE); failsafeState.phase = FAILSAFE_LANDING; - failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND; break; case FAILSAFE_PROCEDURE_DROP_IT: diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7e97cdc417..a0949015bc 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -35,7 +35,7 @@ typedef struct failsafeConfig_s { uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure". uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) - uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) + uint8_t failsafe_landing_time; // Time for Landing before disarm in seconds. uint8_t failsafe_switch_mode; // failsafe switch action is 0: Stage 1, 1: Disarms instantly, 2: Stage 2 uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (min 100ms PERIOD_RXDATA_RECOVERY) to allow recovering from failsafe procedure diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index bb84f3cba7..a9e35378e2 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -79,7 +79,6 @@ typedef struct { float maxAltitudeCm; float returnAltitudeCm; float targetAltitudeCm; - float targetLandingAltitudeCm; float targetVelocityCmS; float pitchAngleLimitDeg; float rollAngleLimitDeg; @@ -241,7 +240,7 @@ static void rescueAttainPosition(void) // 20s of slow descent for switch induced sanity failures to allow time to recover gpsRescueAngle[AI_PITCH] = 0.0f; gpsRescueAngle[AI_ROLL] = 0.0f; - rescueThrottle = gpsRescueConfig()->throttleHover - 100; + rescueThrottle = positionConfig()->hover_throttle - 100; return; default: break; @@ -277,13 +276,13 @@ static void rescueAttainPosition(void) // acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now. float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day - tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000); + tiltAdjustment *= (positionConfig()->hover_throttle - 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 = positionConfig()->hover_throttle + throttleAdjustment; rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, lrintf(throttleP)); @@ -706,7 +705,7 @@ void descend(void) { if (newGPSData) { // consider landing area to be a circle half landing height around home, to avoid overshooting home point - const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (rescueState.intent.targetLandingAltitudeCm / 200.0f); + const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * positionConfig()->landing_altitude_m); const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); // increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home @@ -801,13 +800,12 @@ void gpsRescueUpdate(void) case RESCUE_INITIALIZE: // Things that should be done at the start of a Rescue - rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM; if (!STATE(GPS_FIX_HOME)) { // we didn't get a home point on arming rescueState.failure = RESCUE_NO_HOME_POINT; // will result in a disarm via the sanity check system, or float around if switch induced } else { - if (rescueState.sensor.distanceToHomeM < 5.0f && rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) { + if (rescueState.sensor.distanceToHomeM < 5.0f && isAltitudeLow()) { // attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons rescueState.phase = RESCUE_ABORT; } else { @@ -898,7 +896,7 @@ void gpsRescueUpdate(void) case RESCUE_DESCENT: // attenuate velocity and altitude targets while updating the heading to home - if (rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) { + if (isAltitudeLow()) { // enter landing mode once below landing altitude rescueState.phase = RESCUE_LANDING; rescueState.intent.secondsFailing = 0; // reset sanity timer for landing diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 6ddc3fcd3a..b9945f2545 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -723,7 +723,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) // Update the throttle correction for angle and supply it to the mixer int throttleAngleCorrection = 0; - if (throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)) { + if (throttleAngleValue + && (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE)) + && ARMING_FLAG(ARMED)) { throttleAngleCorrection = calculateThrottleAngleCorrection(); } mixerSetThrottleAngleCorrection(throttleAngleCorrection); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3a6cf1db77..831d895990 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -46,6 +46,7 @@ #include "fc/runtime_config.h" #include "flight/failsafe.h" +#include "flight/alt_hold.h" #include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer_init.h" @@ -724,6 +725,13 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) } #endif +#ifdef USE_ALT_HOLD_MODE + // Throttle value to be used during altitude hold mode (and failsafe landing mode) + if (FLIGHT_MODE(ALT_HOLD_MODE)) { + throttle = altHoldGetThrottle(); + } +#endif + #ifdef USE_GPS_RESCUE // If gps rescue is active then override the throttle. This prevents things // like throttle boost or throttle limit from negatively affecting the throttle. @@ -754,7 +762,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) && ARMING_FLAG(ARMED) && !mixerRuntime.feature3dEnabled && !airmodeEnabled - && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable motor_stop while GPS Rescue is active + && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE) // disable motor_stop while GPS Rescue / Altitude Hold is active && (rcData[THROTTLE] < rxConfig()->mincheck)) { // motor_stop handling applyMotorStop(); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d8f0e9e35c..c741d7afd6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -46,9 +46,11 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/alt_hold.h" #include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/position.h" #include "flight/rpm_filter.h" #include "io/gps.h" @@ -461,7 +463,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ // earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100. sin_approx costs most of those cycles. float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL])); sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f; // must be negative for Roll - const float earthRefGain = FLIGHT_MODE(GPS_RESCUE_MODE) ? 1.0f : pidRuntime.angleEarthRef; + const float earthRefGain = FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE) ? 1.0f : pidRuntime.angleEarthRef; angleRate += pidRuntime.angleYawSetpoint * sinAngle * earthRefGain; pidRuntime.angleTarget[axis] = angleTarget; // set target for alternate axis to current axis, for use in preceding calculation @@ -469,7 +471,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ // this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate); - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) { + if (FLIGHT_MODE(ANGLE_MODE| GPS_RESCUE_MODE)) { currentPidSetpoint = angleRate; } else { // can only be HORIZON mode - crossfade Angle rate and Acro rate @@ -590,7 +592,7 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri { float ret = setPoint; - if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) { + if (!FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE | GPS_RESCUE_MODE | ALT_HOLD_MODE)) { bool resetIterm = false; float projectedAngle = 0; const int setpointSign = acroTrainerSign(setPoint); @@ -805,13 +807,30 @@ float pidGetAirmodeThrottleOffset(void) static FAST_CODE_NOINLINE void disarmOnImpact(void) { - // if all sticks are within 5% of center, and throttle low, check accDelta for impacts - // threshold should be high enough to avoid unwanted disarms in the air on throttle chops - if (isAirmodeActivated() && getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f && - fabsf(acc.accDelta) > pidRuntime.landingDisarmThreshold) { - // disarm on accDelta transients - setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); - disarm(DISARM_REASON_LANDING); + // if, after takeoff... + if (isAirmodeActivated() + // and, either sticks are centred and throttle zeroed, + && ((getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f) + // we could test here for stage 2 failsafe (including both landing or GPS Rescue) + // this may permit removing the GPS Rescue disarm method altogether +#ifdef USE_ALT_HOLD_MODE + // or in altitude hold mode, including failsafe landing mode, indirectly + || FLIGHT_MODE(ALT_HOLD_MODE) +#endif + )) { + // increase sensitivity by 50% when low and in altitude hold or failsafe landing + // for more reliable disarm with gentle controlled landings + float lowAltitudeSensitivity = 1.0f; +#ifdef USE_ALT_HOLD_MODE + lowAltitudeSensitivity = (FLIGHT_MODE(ALT_HOLD_MODE) && isAltitudeLow()) ? 1.5f : 1.0f; +#endif + // and disarm if accelerometer jerk exceeds threshold... + if ((fabsf(acc.accDelta) * lowAltitudeSensitivity) > pidRuntime.landingDisarmThreshold) { + // then disarm + setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); // NB: need a better message + disarm(DISARM_REASON_LANDING); + // note: threshold should be high enough to avoid unwanted disarms in the air on throttle chops, eg around 10 + } } DEBUG_SET(DEBUG_EZLANDING, 6, lrintf(getMaxRcDeflectionAbs() * 100.0f)); DEBUG_SET(DEBUG_EZLANDING, 7, lrintf(acc.accDelta)); @@ -946,14 +965,18 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim #if defined(USE_ACC) static timeUs_t levelModeStartTimeUs = 0; - static bool gpsRescuePreviousState = false; + static bool prevExternalAngleRequest = false; const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims; float horizonLevelStrength = 0.0f; - const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE); + const bool isExternalAngleModeRequest = FLIGHT_MODE(GPS_RESCUE_MODE) +#ifdef USE_ALT_HOLD_MODE + || FLIGHT_MODE(ALT_HOLD_MODE) +#endif + ; levelMode_e levelMode; - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive) { - if (pidRuntime.levelRaceMode && !gpsRescueIsActive) { + if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) { + if (pidRuntime.levelRaceMode && !isExternalAngleModeRequest) { levelMode = LEVEL_MODE_R; } else { levelMode = LEVEL_MODE_RP; @@ -962,7 +985,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim // Keep track of when we entered a self-level mode so that we can // add a guard time before crash recovery can activate. // Also reset the guard time whenever GPS Rescue is activated. - if ((levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)) { + if ((levelModeStartTimeUs == 0) || (isExternalAngleModeRequest && !prevExternalAngleRequest)) { levelModeStartTimeUs = currentTimeUs; } @@ -975,7 +998,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim levelModeStartTimeUs = 0; } - gpsRescuePreviousState = gpsRescueIsActive; + prevExternalAngleRequest = isExternalAngleModeRequest; #else UNUSED(pidProfile); UNUSED(currentTimeUs); diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 8552f3d766..d80b78b830 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -49,13 +49,17 @@ static float displayAltitudeCm = 0.0f; static float zeroedAltitudeCm = 0.0f; +static bool altitudeAvailable = false; +static bool altitudeIsLow = false; #if defined(USE_BARO) || defined(USE_GPS) + static float zeroedAltitudeDerivative = 0.0f; #endif static pt2Filter_t altitudeLpf; static pt2Filter_t altitudeDerivativeLpf; + #ifdef USE_VARIO static int16_t estimatedVario = 0; // in cm/s #endif @@ -79,13 +83,15 @@ typedef enum { GPS_ONLY } altitudeSource_e; -PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 5); PG_RESET_TEMPLATE(positionConfig_t, positionConfig, .altitude_source = DEFAULT, .altitude_prefer_baro = 100, // percentage 'trust' of baro data .altitude_lpf = 300, .altitude_d_lpf = 100, + .hover_throttle = 1275, + .landing_altitude_m = 4, ); #if defined(USE_BARO) || defined(USE_GPS) @@ -115,7 +121,7 @@ void calculateEstimatedAltitude(void) // GPS_FIX means a 3D fix, which requires min 4 sats. // On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero. gpsAltCm = gpsSol.llh.altCm; // static, so hold last altitude value if 3D fix is lost to prevent fly to moon - haveGpsAlt = true; // stays false if no 3D fix + haveGpsAlt = true; // goes false and stays false if no 3D fix if (gpsSol.dop.pdop != 0) { // pDOP of 1.0 is good. 100 is very bad. Our gpsSol.dop.pdop values are *100 // When pDOP is a value less than 3.3, GPS trust will be stronger than default. @@ -196,6 +202,9 @@ void calculateEstimatedAltitude(void) zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s previousZeroedAltitudeCm = zeroedAltitudeCm; + // assess if altitude is low here, only when we get new data, rather than in pid loop etc + altitudeIsLow = zeroedAltitudeCm < 100.0f * positionConfig()->landing_altitude_m; + zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative); #ifdef USE_VARIO @@ -210,9 +219,15 @@ void calculateEstimatedAltitude(void) DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario); #endif DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f)); + + altitudeAvailable = haveGpsAlt || haveBaroAlt; } #endif //defined(USE_BARO) || defined(USE_GPS) +bool isAltitudeAvailable(void) { + return altitudeAvailable; +} + int32_t getEstimatedAltitudeCm(void) { return lrintf(displayAltitudeCm); @@ -223,6 +238,11 @@ float getAltitude(void) return zeroedAltitudeCm; } +bool isAltitudeLow(void) +{ + return altitudeIsLow; +} + #ifdef USE_GPS float getAltitudeAsl(void) { diff --git a/src/main/flight/position.h b/src/main/flight/position.h index 8084b4c97f..0e349ea287 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -29,6 +29,8 @@ typedef struct positionConfig_s { uint8_t altitude_prefer_baro; uint16_t altitude_lpf; // lowpass cutoff (value / 100) Hz for altitude smoothing uint16_t altitude_d_lpf; // lowpass for (value / 100) Hz for altitude derivative smoothing + uint16_t hover_throttle; // value used at the start of a rescue or position hold + uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres } positionConfig_t; PG_DECLARE(positionConfig_t, positionConfig); @@ -37,5 +39,7 @@ void calculateEstimatedAltitude(void); void positionInit(void); int32_t getEstimatedAltitudeCm(void); float getAltitude(void); +bool isAltitudeLow(void); float getAltitudeAsl(void); int16_t getEstimatedVario(void); +bool isAltitudeAvailable(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 6a7b5155ea..0ff46c3f59 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1545,7 +1545,7 @@ case MSP_NAME: sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS); sbufWriteU16(dst, gpsRescueConfig()->throttleMin); sbufWriteU16(dst, gpsRescueConfig()->throttleMax); - sbufWriteU16(dst, gpsRescueConfig()->throttleHover); + sbufWriteU16(dst, positionConfig()->hover_throttle); sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); sbufWriteU8(dst, gpsRescueConfig()->minSats); @@ -1654,7 +1654,7 @@ case MSP_NAME: break; case MSP_FAILSAFE_CONFIG: sbufWriteU8(dst, failsafeConfig()->failsafe_delay); - sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay); + sbufWriteU8(dst, failsafeConfig()->failsafe_landing_time); sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode); sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay); @@ -2880,7 +2880,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src); gpsRescueConfigMutable()->throttleMin = sbufReadU16(src); gpsRescueConfigMutable()->throttleMax = sbufReadU16(src); - gpsRescueConfigMutable()->throttleHover = sbufReadU16(src); + positionConfigMutable()->hover_throttle = sbufReadU16(src); gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); gpsRescueConfigMutable()->minSats = sbufReadU8(src); if (sbufBytesRemaining(src) >= 6) { @@ -3811,7 +3811,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, break; case MSP_SET_FAILSAFE_CONFIG: failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); - failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_landing_time = sbufReadU8(src); failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src); failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index 0fe27fd3bc..4431808974 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -50,7 +50,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { .boxId = BOXARM, .boxName = "ARM", .permanentId = 0 }, { .boxId = BOXANGLE, .boxName = "ANGLE", .permanentId = 1 }, { .boxId = BOXHORIZON, .boxName = "HORIZON", .permanentId = 2 }, -// { .boxId = BOXBARO, .boxName = "BARO", .permanentId = 3 }, + { .boxId = BOXALTHOLD, .boxName = "ALTHOLD", .permanentId = 3 }, { .boxId = BOXANTIGRAVITY, .boxName = "ANTI GRAVITY", .permanentId = 4 }, { .boxId = BOXMAG, .boxName = "MAG", .permanentId = 5 }, { .boxId = BOXHEADFREE, .boxName = "HEADFREE", .permanentId = 6 }, @@ -205,6 +205,9 @@ void initActiveBoxIds(void) if (sensors(SENSOR_ACC)) { BME(BOXANGLE); BME(BOXHORIZON); +#ifdef USE_ALT_HOLD_MODE + BME(BOXALTHOLD); +#endif BME(BOXHEADFREE); BME(BOXHEADADJ); BME(BOXFPVANGLEMIX); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 349b7397dd..c06a084057 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -1048,7 +1048,7 @@ static void osdElementFlymode(osdElementParms_t *element) // Note that flight mode display has precedence in what to display. // 1. FS // 2. GPS RESCUE - // 3. ANGLE, HORIZON, ACRO TRAINER + // 3. ANGLE, HORIZON, ACRO TRAINER, ALTHOLD // 4. AIR // 5. ACRO @@ -1060,6 +1060,8 @@ static void osdElementFlymode(osdElementParms_t *element) strcpy(element->buff, "HEAD"); } else if (FLIGHT_MODE(ANGLE_MODE)) { strcpy(element->buff, "ANGL"); + } else if (FLIGHT_MODE(ALT_HOLD_MODE)) { + strcpy(element->buff, "ALTH "); } else if (FLIGHT_MODE(HORIZON_MODE)) { strcpy(element->buff, "HOR "); } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) { diff --git a/src/main/pg/alt_hold.c b/src/main/pg/alt_hold.c new file mode 100644 index 0000000000..ddf582d57f --- /dev/null +++ b/src/main/pg/alt_hold.c @@ -0,0 +1,42 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_ALT_HOLD_MODE + +#include "flight/alt_hold.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "alt_hold.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 3); + +PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig, + .alt_hold_pid_p = 15, + .alt_hold_pid_i = 15, + .alt_hold_pid_d = 15, + .alt_hold_target_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s + .alt_hold_throttle_min = 1100, + .alt_hold_throttle_max = 1700, +); +#endif diff --git a/src/main/pg/alt_hold.h b/src/main/pg/alt_hold.h new file mode 100644 index 0000000000..93d7db398d --- /dev/null +++ b/src/main/pg/alt_hold.h @@ -0,0 +1,37 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include + +#include "pg/pg.h" + +typedef struct altholdConfig_s { + uint8_t alt_hold_pid_p; + uint8_t alt_hold_pid_i; + uint8_t alt_hold_pid_d; + uint8_t alt_hold_target_adjust_rate; + uint16_t alt_hold_throttle_min; + uint16_t alt_hold_throttle_max; +} altholdConfig_t; + +PG_DECLARE(altholdConfig_t, altholdConfig); + diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c index 71ed946c7f..24e498438e 100644 --- a/src/main/pg/gps_rescue.c +++ b/src/main/pg/gps_rescue.c @@ -29,7 +29,7 @@ #include "gps_rescue.h" -PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 5); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 6); PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, @@ -46,13 +46,11 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .descentDistanceM = 20, .descendRate = 150, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent - .targetLandingAltitudeM = 4, .disarmThreshold = 30, .throttleMin = 1100, .throttleMax = 1700, - .throttleHover = 1275, - + .allowArmingWithoutFix = false, .sanityChecks = RESCUE_SANITY_FS_ONLY, .minSats = 8, diff --git a/src/main/pg/gps_rescue.h b/src/main/pg/gps_rescue.h index b9c5eed4f8..14fe451e96 100644 --- a/src/main/pg/gps_rescue.h +++ b/src/main/pg/gps_rescue.h @@ -34,14 +34,12 @@ typedef struct gpsRescue_s { uint8_t yawP; uint16_t throttleMin; uint16_t throttleMax; - uint16_t throttleHover; uint8_t minSats; uint8_t velP, velI, velD; uint16_t minStartDistM; // meters uint8_t sanityChecks; uint8_t allowArmingWithoutFix; uint8_t useMag; - uint8_t targetLandingAltitudeM; // meters uint8_t altitudeMode; uint16_t ascendRate; uint16_t descendRate; diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 7a97873c54..f01b339eb7 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -82,6 +82,7 @@ #define PG_POSITION 56 #define PG_VTX_IO_CONFIG 57 #define PG_GPS_LAP_TIMER 58 +#define PG_ALTHOLD_CONFIG 59 // Driver configuration #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 972135a16f..394d29fe39 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -119,6 +119,9 @@ typedef enum { #ifdef USE_GPS_RESCUE TASK_GPS_RESCUE, #endif +#ifdef USE_ALT_HOLD_MODE + TASK_ALTHOLD, +#endif #ifdef USE_MAG TASK_COMPASS, #endif diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 9cb75fe74e..7a83adceb7 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -245,6 +245,7 @@ #define USE_EMFAT_AUTORUN #define USE_EMFAT_ICON #define USE_ESCSERIAL_SIMONK +#define USE_ALT_HOLD_MODE #if !defined(USE_GPS) #define USE_GPS diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c39abce780..d3e3820419 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -406,6 +406,8 @@ void crsfFrameFlightMode(sbuf_t *dst) flightMode = "MANU"; } else if (FLIGHT_MODE(ANGLE_MODE)) { flightMode = "STAB"; + } else if (FLIGHT_MODE(ALT_HOLD_MODE)) { + flightMode = "ALTH "; } else if (FLIGHT_MODE(HORIZON_MODE)) { flightMode = "HOR"; } else if (airmodeIsEnabled()) { diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index d7862bbc06..70c279f221 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -254,13 +254,13 @@ static uint16_t getRPM(void) static uint16_t getMode(void) { uint16_t flightMode = 1; //Acro - if (FLIGHT_MODE(ANGLE_MODE)) { + if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE)) { flightMode = 0; //Stab } if (FLIGHT_MODE(PASSTHRU_MODE)) { flightMode = 3; //Auto } - if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE)) { + if (FLIGHT_MODE(HEADFREE_MODE | MAG_MODE)) { flightMode = 4; //Guided! (there in no HEAD, MAG so use Guided) } if (FLIGHT_MODE(HORIZON_MODE)) { diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 75b59514b2..222d8753d6 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -176,6 +176,8 @@ static void ltm_sframe(void) lt_flightmode = 2; else if (FLIGHT_MODE(HORIZON_MODE)) lt_flightmode = 3; + else if (FLIGHT_MODE(ALT_HOLD_MODE)) + lt_flightmode = 5; else lt_flightmode = 1; // Rate mode diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 130ac6d162..c9c79d9199 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -478,7 +478,7 @@ void mavlinkSendHUDAndHeartbeat(void) // Custom mode for compatibility with APM OSDs uint8_t mavCustomMode = 1; // Acro by default - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE | ALT_HOLD_MODE)) { mavCustomMode = 0; //Stabilize mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 7f1ebbbdf5..71d0530b1b 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -771,7 +771,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear tmpi += 4; } - if (FLIGHT_MODE(ANGLE_MODE)) { + if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE)) { tmpi += 10; } if (FLIGHT_MODE(HORIZON_MODE)) { diff --git a/src/test/Makefile b/src/test/Makefile index 0ec3a16f74..89ac6c5828 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -458,6 +458,14 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \ USE_GPS= \ USE_MSP_OVER_TELEMETRY= \ +althold_unittest_SRC := \ + $(USER_DIR)/flight/alt_hold.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/pg/rx.c + +althold_unittest_DEFINES := \ + USE_ALT_HOLD_MODE= \ + vtx_msp_unittest_SRC := \ $(USER_DIR)/common/crc.c \ $(USER_DIR)/common/streambuf.c \ diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc new file mode 100644 index 0000000000..1401363f7b --- /dev/null +++ b/src/test/unit/althold_unittest.cc @@ -0,0 +1,134 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + + +#include +#include +#include + +extern "C" { + + #include "platform.h" + #include "build/debug.h" + #include "pg/pg_ids.h" + + #include "fc/core.h" + #include "fc/rc_controls.h" + #include "fc/runtime_config.h" + + #include "flight/alt_hold.h" + #include "flight/failsafe.h" + #include "flight/imu.h" + #include "flight/position.h" + + #include "rx/rx.h" + + #include "sensors/acceleration.h" + + PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); + PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); + PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0); + + extern altHoldState_t altHoldState; + void altHoldReset(void); + void altHoldProcessTransitions(void); + void altHoldInit(void); + void updateAltHoldState(timeUs_t); + bool failsafeIsActive(void) { return false; } + timeUs_t currentTimeUs = 0; +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +uint32_t millisRW; +uint32_t millis() { + return millisRW; +} + +TEST(AltholdUnittest, altHoldTransitionsTest) +{ + updateAltHoldState(currentTimeUs); + EXPECT_EQ(altHoldState.isAltHoldActive, false); + + flightModeFlags |= ALT_HOLD_MODE; + millisRW = 42; + updateAltHoldState(currentTimeUs); + EXPECT_EQ(altHoldState.isAltHoldActive, true); + + flightModeFlags ^= ALT_HOLD_MODE; + millisRW = 56; + updateAltHoldState(currentTimeUs); + EXPECT_EQ(altHoldState.isAltHoldActive, false); + + flightModeFlags |= ALT_HOLD_MODE; + millisRW = 64; + updateAltHoldState(currentTimeUs); + EXPECT_EQ(altHoldState.isAltHoldActive, true); +} + +TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter) +{ + altHoldInit(); + EXPECT_EQ(altHoldState.isAltHoldActive, false); + + flightModeFlags |= ALT_HOLD_MODE; + millisRW = 42; + updateAltHoldState(currentTimeUs); + EXPECT_EQ(altHoldState.isAltHoldActive, true); +} + +// STUBS + +extern "C" { + acc_t acc; + + void pt2FilterInit(pt2Filter_t *altHoldDeltaLpf, float) { + UNUSED(altHoldDeltaLpf); + } + float pt2FilterGain(float, float) { + return 0.0f; + } + float pt2FilterApply(pt2Filter_t *altHoldDeltaLpf, float) { + UNUSED(altHoldDeltaLpf); + return 0.0f; + } + + bool isAltitudeAvailable(void) { return true; } + float getAltitude(void) { return 0.0f; } + bool isAltitudeLow(void) { return true; } + float getCosTiltAngle(void) { return 0.0f; } + float rcCommand[4]; + + float getRcDeflection(int) + { + return 0; + } + + void parseRcChannels(const char *input, rxConfig_t *rxConfig) + { + UNUSED(input); + UNUSED(rxConfig); + } + + int16_t debug[DEBUG16_VALUE_COUNT]; + uint8_t debugMode; + + uint8_t armingFlags = 0; + uint8_t stateFlags = 0; + uint16_t flightModeFlags = 0; +} diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 7e51c4e083..19bef4ff08 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1160,5 +1160,6 @@ extern "C" { return 0.0f; } void getRcDeflectionAbs(void) {} - uint32_t getCpuPercentageLate(void) { return 0; }; + uint32_t getCpuPercentageLate(void) { return 0; } + bool isAltitudeLow(void) {return false ;}; } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 3f6ce0f082..2393bc0112 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -81,7 +81,7 @@ void configureFailsafe(void) rxConfigMutable()->mincheck = TEST_MIN_CHECK; failsafeConfigMutable()->failsafe_delay = 10; // 1 second - failsafeConfigMutable()->failsafe_off_delay = 15; // 1.5 seconds + failsafeConfigMutable()->failsafe_landing_time = 1; // 1.0 seconds failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1; failsafeConfigMutable()->failsafe_throttle = 1200; failsafeConfigMutable()->failsafe_throttle_low_delay = 100; // 10 seconds @@ -233,7 +233,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) // note this test follows on from the previous test { // exceed the stage 2 landing time - sysTickUptime += (failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND); + sysTickUptime += (failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND); failsafeOnValidDataFailed(); // confirm that we still have no valid data // when @@ -572,8 +572,8 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land) EXPECT_EQ(FAILSAFE_LANDING, failsafePhase()); EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM)); - // should stay in landing for failsafe_off_delay (stage 2 period) of 1s - sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; + // should stay in landing for failsafe_landing_time (stage 2 landing period) of 1s + sysTickUptime += failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND; // when failsafeUpdateState(); diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index ae565beefb..97f6c06154 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -65,7 +65,8 @@ extern "C" { #include "flight/mixer.h" #include "flight/pid.h" #include "flight/pid_init.h" - + #include "flight/position.h" + #include "io/gps.h" #include "pg/pg.h" @@ -85,6 +86,7 @@ extern "C" { PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); + PG_REGISTER(positionConfig_t, positionConfig, PG_SYSTEM_CONFIG, 4); bool unitLaunchControlActive = false; launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL; @@ -94,10 +96,13 @@ extern "C" { bool isAirmodeActivated(void) { return simulatedAirmodeEnabled; } float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); } - // used by ezDisarm auto-disarm code + // used by auto-disarm code float getMaxRcDeflectionAbs() { return fabsf(simulatedMaxRcDeflectionAbs); } float mixerGetRcThrottle() { return fabsf(simulatedMixerGetRcThrottle); } + + bool isAltitudeLow(void) { return false; } + void systemBeep(bool) { } bool gyroOverflowDetected(void) { return false; } float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }