Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-12 19:10:32 +03:00
Code Issues Wiki Activity

Altitude hold for 4.6 (#13816)

Browse source
This commit is contained in:
ctzsnooze 2024-09-04 20:29:03 +10:00 • committed by GitHub
parent 350510603c
commit 254da8f460
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
46 changed files with 805 additions and 108 deletions
Download patch file Download diff file Expand all files Collapse all files

2
mk/source.mk
View file

@ -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 \

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

@ -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,11 +1695,19 @@ 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

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

@ -121,4 +121,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"S_TERM",
"SPA",
"TASK",
"ALTHOLD",
};

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

@ -123,6 +123,7 @@ typedef enum {
DEBUG_S_TERM,
DEBUG_SPA,
DEBUG_TASK,
DEBUG_ALTHOLD,
DEBUG_COUNT
} debugType_e;

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

@ -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
@ -1817,10 +1826,12 @@ const clivalue_t valueTable[] = {
#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)

8
src/main/cms/cms_menu_failsafe.c
View file

@ -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},

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

@ -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 },

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

@ -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));

5
src/main/fc/init.c
View file

@ -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();

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

@ -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"

4
src/main/fc/rc.c
View file

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

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

@ -303,7 +303,6 @@ void processRcStickPositions(void)
}
#endif
if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE)) {
// in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
rollAndPitchTrims_t accelerometerTrimsDelta;

2
src/main/fc/rc_modes.c
View file

@ -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);

1
src/main/fc/rc_modes.h
View file

@ -33,6 +33,7 @@ typedef enum {
BOXANGLE,
BOXHORIZON,
BOXMAG,
BOXALTHOLD,
BOXHEADFREE,
BOXPASSTHRU,
BOXFAILSAFE,

3
src/main/fc/runtime_config.h
View file

@ -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), \

9
src/main/fc/tasks.c
View file

@ -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

287
src/main/flight/alt_hold.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

41
src/main/flight/alt_hold.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

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

@ -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:

2
src/main/flight/failsafe.h
View file

@ -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

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

@ -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

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

@ -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);

10
src/main/flight/mixer.c
View file

@ -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();

53
src/main/flight/pid.c
View file

@ -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);
// 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);

24
src/main/flight/position.c
View file

@ -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)
{

4
src/main/flight/position.h
View file

@ -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);

8
src/main/msp/msp.c
View file

@ -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);

5
src/main/msp/msp_box.c
View file

@ -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);

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

@ -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)) {

42
src/main/pg/alt_hold.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

37
src/main/pg/alt_hold.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#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);

4
src/main/pg/gps_rescue.c
View file

@ -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,12 +46,10 @@ 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,

2
src/main/pg/gps_rescue.h
View file

@ -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;

1
src/main/pg/pg_ids.h
View file

@ -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

3
src/main/scheduler/scheduler.h
View file

@ -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

1
src/main/target/common_pre.h
View file

@ -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

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

@ -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()) {

4
src/main/telemetry/ibus_shared.c
View file

@ -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)) {

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

@ -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

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

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

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

@ -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)) {

8
src/test/Makefile
View file

@ -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 \

134
src/test/unit/althold_unittest.cc Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include <limits.h>
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;
}

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

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

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

@ -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();

7
src/test/unit/pid_unittest.cc
View file

@ -65,6 +65,7 @@ extern "C" {
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/position.h"
#include "io/gps.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]; }