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