1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 03:20:00 +03:00

Shared altitude control function in position_control.c (#13954)

* Altitude control code shared

fewer debugs
subtract D

* remove #include comments, simplify coding, restructuring

thanks JP and MH

* fix cms limits for throttle control

* Use altitude_control debug, fix throttle calculation

minor refactoring

* use AUTO_CONTROL_ALTITUDE debug in place of GPS Rescue throttle pid

* use autopilot for position control names

* fixes from reviews - thanks

* Re-organise included files and functions thanks Karate

* missed init and other typos

* remove old unused unit test file, tidy up thanks Mark

* fix indentation on one line
This commit is contained in:
ctzsnooze 2024-10-17 08:17:44 +11:00 committed by GitHub
parent 1d9823b4cd
commit 58fc8bbbb8
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
33 changed files with 263 additions and 595 deletions

View file

@ -1,6 +1,7 @@
PG_SRC = \ PG_SRC = \
pg/adc.c \ pg/adc.c \
pg/alt_hold.c \ pg/alt_hold.c \
pg/autopilot.c \
pg/beeper.c \ pg/beeper.c \
pg/beeper_dev.c \ pg/beeper_dev.c \
pg/board.c \ pg/board.c \
@ -21,7 +22,6 @@ PG_SRC = \
pg/msp.c \ pg/msp.c \
pg/pg.c \ pg/pg.c \
pg/piniobox.c \ pg/piniobox.c \
pg/position_control.c \
pg/pinio.c \ pg/pinio.c \
pg/pin_pull_up_down.c \ pg/pin_pull_up_down.c \
pg/rcdevice.c \ pg/rcdevice.c \
@ -152,23 +152,23 @@ COMMON_SRC = \
drivers/rx/rx_pwm.c \ drivers/rx/rx_pwm.c \
drivers/serial_softserial.c \ drivers/serial_softserial.c \
fc/core.c \ fc/core.c \
fc/gps_lap_timer.c \
fc/rc.c \ fc/rc.c \
fc/rc_adjustments.c \ fc/rc_adjustments.c \
fc/rc_controls.c \ fc/rc_controls.c \
fc/rc_modes.c \ fc/rc_modes.c \
flight/position.c \ flight/alt_hold.c \
flight/position_control.c \ flight/autopilot.c \
flight/dyn_notch_filter.c \
flight/failsafe.c \ flight/failsafe.c \
flight/gps_rescue.c \ flight/gps_rescue.c \
fc/gps_lap_timer.c \
flight/dyn_notch_filter.c \
flight/alt_hold.c \
flight/imu.c \ flight/imu.c \
flight/mixer.c \ flight/mixer.c \
flight/mixer_init.c \ flight/mixer_init.c \
flight/mixer_tricopter.c \ flight/mixer_tricopter.c \
flight/pid.c \ flight/pid.c \
flight/pid_init.c \ flight/pid_init.c \
flight/position.c \
flight/rpm_filter.c \ flight/rpm_filter.c \
flight/servos.c \ flight/servos.c \
flight/servos_tricopter.c \ flight/servos_tricopter.c \

View file

@ -63,12 +63,12 @@
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/alt_hold.h" #include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/servos.h" #include "flight/servos.h"
@ -1616,12 +1616,15 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro); 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_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_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", positionControlConfig()->hover_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", positionControlConfig()->landing_altitude_m); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", autopilotConfig()->landing_altitude_m);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", positionControlConfig()->altitude_P); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", autopilotConfig()->hover_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", positionControlConfig()->altitude_I);; BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", autopilotConfig()->throttle_min);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", positionControlConfig()->altitude_D);; BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", autopilotConfig()->throttle_max);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", positionControlConfig()->altitude_F); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", autopilotConfig()->altitude_P);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", autopilotConfig()->altitude_I);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", autopilotConfig()->altitude_D);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", autopilotConfig()->altitude_F);
#ifdef USE_MAG #ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
@ -1713,9 +1716,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold) 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_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
@ -1732,8 +1732,6 @@ static bool blackboxWriteSysinfo(void)
#endif // USE_GPS #endif // USE_GPS
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
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); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, "%d", altholdConfig()->alt_hold_target_adjust_rate);
#endif #endif

View file

@ -83,7 +83,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
[DEBUG_CRSF_LINK_STATISTICS_PWR] = "CRSF_LINK_STATISTICS_PWR", [DEBUG_CRSF_LINK_STATISTICS_PWR] = "CRSF_LINK_STATISTICS_PWR",
[DEBUG_CRSF_LINK_STATISTICS_DOWN] = "CRSF_LINK_STATISTICS_DOWN", [DEBUG_CRSF_LINK_STATISTICS_DOWN] = "CRSF_LINK_STATISTICS_DOWN",
[DEBUG_BARO] = "BARO", [DEBUG_BARO] = "BARO",
[DEBUG_GPS_RESCUE_THROTTLE_PID] = "GPS_RESCUE_THROTTLE_PID", [DEBUG_AUTOPILOT_ALTITUDE] = "AUTOPILOT_ALTITUDE",
[DEBUG_DYN_IDLE] = "DYN_IDLE", [DEBUG_DYN_IDLE] = "DYN_IDLE",
[DEBUG_FEEDFORWARD_LIMIT] = "FEEDFORWARD_LIMIT", [DEBUG_FEEDFORWARD_LIMIT] = "FEEDFORWARD_LIMIT",
[DEBUG_FEEDFORWARD] = "FEEDFORWARD", [DEBUG_FEEDFORWARD] = "FEEDFORWARD",
@ -121,5 +121,4 @@ const char * const debugModeNames[DEBUG_COUNT] = {
[DEBUG_S_TERM] = "S_TERM", [DEBUG_S_TERM] = "S_TERM",
[DEBUG_SPA] = "SPA", [DEBUG_SPA] = "SPA",
[DEBUG_TASK] = "TASK", [DEBUG_TASK] = "TASK",
[DEBUG_ALTHOLD] = "ALTHOLD",
}; };

View file

@ -85,7 +85,7 @@ typedef enum {
DEBUG_CRSF_LINK_STATISTICS_PWR, DEBUG_CRSF_LINK_STATISTICS_PWR,
DEBUG_CRSF_LINK_STATISTICS_DOWN, DEBUG_CRSF_LINK_STATISTICS_DOWN,
DEBUG_BARO, DEBUG_BARO,
DEBUG_GPS_RESCUE_THROTTLE_PID, DEBUG_AUTOPILOT_ALTITUDE,
DEBUG_DYN_IDLE, DEBUG_DYN_IDLE,
DEBUG_FEEDFORWARD_LIMIT, DEBUG_FEEDFORWARD_LIMIT,
DEBUG_FEEDFORWARD, DEBUG_FEEDFORWARD,
@ -123,7 +123,6 @@ typedef enum {
DEBUG_S_TERM, DEBUG_S_TERM,
DEBUG_SPA, DEBUG_SPA,
DEBUG_TASK, DEBUG_TASK,
DEBUG_ALTHOLD,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

View file

@ -104,12 +104,12 @@ bool cliMode = false;
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"

View file

@ -56,14 +56,14 @@
#include "fc/rc_adjustments.h" #include "fc/rc_adjustments.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/alt_hold.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/servos.h" #include "flight/servos.h"
@ -1071,17 +1071,14 @@ const clivalue_t valueTable[] = {
{ 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_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
{ 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_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_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) }, { PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
{ PARAM_NAME_GPS_RESCUE_MIN_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, { PARAM_NAME_GPS_RESCUE_MIN_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
{ PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) }, { PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
{ PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) }, { PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) },
{ PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) }, { PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
{ PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) }, { PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
{ PARAM_NAME_GPS_RESCUE_YAW_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) }, { PARAM_NAME_GPS_RESCUE_YAW_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) },
#ifdef USE_MAG #ifdef USE_MAG
{ PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) }, { PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
@ -1102,8 +1099,6 @@ const clivalue_t valueTable[] = {
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, { "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 #ifdef USE_ALT_HOLD_MODE
{ 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) }, { 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 #endif
@ -1835,13 +1830,15 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_ALTITUDE_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_lpf) }, { 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_ALTITUDE_D_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) },
// PG_POSITION_CONTROL // PG_AUTOPILOT
{ PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, hover_throttle) }, { PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, landing_altitude_m) },
{ PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, landing_altitude_m) }, { PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, hover_throttle) },
{ PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_P) }, { PARAM_NAME_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttle_min) },
{ PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_I) }, { PARAM_NAME_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttle_max) },
{ PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_D) }, { PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_P) },
{ PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_F) }, { PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_I) },
{ PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_D) },
{ PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_F) },
// PG_MODE_ACTIVATION_CONFIG // PG_MODE_ACTIVATION_CONFIG
#if defined(USE_CUSTOM_BOX_NAMES) #if defined(USE_CUSTOM_BOX_NAMES)

View file

@ -37,9 +37,9 @@
#include "config/config.h" #include "config/config.h"
#include "flight/autopilot.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
static uint16_t gpsRescueConfig_minStartDistM; //meters static uint16_t gpsRescueConfig_minStartDistM; //meters
static uint8_t gpsRescueConfig_altitudeMode; static uint8_t gpsRescueConfig_altitudeMode;
@ -52,16 +52,16 @@ static uint8_t gpsRescueConfig_angle; //degrees
static uint16_t gpsRescueConfig_descentDistanceM; //meters static uint16_t gpsRescueConfig_descentDistanceM; //meters
static uint16_t gpsRescueConfig_descendRate; static uint16_t gpsRescueConfig_descendRate;
static uint8_t positionControlConfig_landingAltitudeM; static uint8_t autopilotConfig_landingAltitudeM;
static uint16_t gpsRescueConfig_throttleMin; static uint16_t autopilotConfig_throttleMin;
static uint16_t gpsRescueConfig_throttleMax; static uint16_t autopilotConfig_throttleMax;
static uint16_t positionControlConfig_hoverThrottle; static uint16_t autopilotConfig_hoverThrottle;
static uint8_t gpsRescueConfig_minSats; static uint8_t gpsRescueConfig_minSats;
static uint8_t gpsRescueConfig_allowArmingWithoutFix; static uint8_t gpsRescueConfig_allowArmingWithoutFix;
static uint8_t positionControlConfig_altitude_P, positionControlConfig_altitude_I, positionControlConfig_altitude_D, positionControlConfig_altitude_F; static uint8_t autopilotConfig_altitude_P, autopilotConfig_altitude_I, autopilotConfig_altitude_D, autopilotConfig_altitude_F;
static uint8_t gpsRescueConfig_yawP; static uint8_t gpsRescueConfig_yawP;
static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD; static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
@ -72,10 +72,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
{ {
UNUSED(pDisp); UNUSED(pDisp);
positionControlConfig_altitude_P = positionControlConfig()->altitude_P; autopilotConfig_altitude_P = autopilotConfig()->altitude_P;
positionControlConfig_altitude_I = positionControlConfig()->altitude_I; autopilotConfig_altitude_I = autopilotConfig()->altitude_I;
positionControlConfig_altitude_D = positionControlConfig()->altitude_D; autopilotConfig_altitude_D = autopilotConfig()->altitude_D;
positionControlConfig_altitude_F = positionControlConfig()->altitude_F; autopilotConfig_altitude_F = autopilotConfig()->altitude_F;
gpsRescueConfig_yawP = gpsRescueConfig()->yawP; gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
@ -94,10 +94,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
UNUSED(pDisp); UNUSED(pDisp);
UNUSED(self); UNUSED(self);
positionControlConfigMutable()->altitude_P = positionControlConfig_altitude_P; autopilotConfigMutable()->altitude_P = autopilotConfig_altitude_P;
positionControlConfigMutable()->altitude_I = positionControlConfig_altitude_I; autopilotConfigMutable()->altitude_I = autopilotConfig_altitude_I;
positionControlConfigMutable()->altitude_D = positionControlConfig_altitude_D; autopilotConfigMutable()->altitude_D = autopilotConfig_altitude_D;
positionControlConfigMutable()->altitude_F = positionControlConfig_altitude_F; autopilotConfigMutable()->altitude_F = autopilotConfig_altitude_F;
gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP; gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
@ -115,16 +115,16 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
{ {
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL}, {"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
{ "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_P, 0, 255, 1 } }, { "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_P, 0, 200, 1 } },
{ "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_I, 0, 255, 1 } }, { "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_I, 0, 200, 1 } },
{ "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_D, 0, 255, 1 } }, { "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_D, 0, 200, 1 } },
{ "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_F, 0, 255, 1 } }, { "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_F, 0, 200, 1 } },
{ "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 255, 1 } }, { "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 200, 1 } },
{ "VELOCITY P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velP, 0, 255, 1 } }, { "VELOCITY P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velP, 0, 200, 1 } },
{ "VELOCITY I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 255, 1 } }, { "VELOCITY I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 200, 1 } },
{ "VELOCITY D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 255, 1 } }, { "VELOCITY D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 200, 1 } },
{ "SMOOTHING", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } }, { "SMOOTHING", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } },
{ "IMU_YAW_GAIN", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_imuYawGain, 5, 20, 1 } }, { "IMU_YAW_GAIN", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_imuYawGain, 5, 20, 1 } },
@ -159,11 +159,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM; gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate; gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
positionControlConfig_landingAltitudeM = positionControlConfig()->landing_altitude_m; autopilotConfig_landingAltitudeM = autopilotConfig()->landing_altitude_m;
gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin; autopilotConfig_throttleMin = autopilotConfig()->throttle_min;
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax; autopilotConfig_throttleMax = autopilotConfig()->throttle_max;
positionControlConfig_hoverThrottle = positionControlConfig()->hover_throttle; autopilotConfig_hoverThrottle = autopilotConfig()->hover_throttle;
gpsRescueConfig_minSats = gpsRescueConfig()->minSats; gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
@ -187,11 +187,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM; gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate; gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
positionControlConfigMutable()->landing_altitude_m = positionControlConfig_landingAltitudeM; autopilotConfigMutable()->landing_altitude_m = autopilotConfig_landingAltitudeM;
gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin; autopilotConfigMutable()->throttle_min = autopilotConfig_throttleMin;
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax; autopilotConfigMutable()->throttle_max = autopilotConfig_throttleMax;
positionControlConfigMutable()->hover_throttle = positionControlConfig_hoverThrottle; autopilotConfigMutable()->hover_throttle = autopilotConfig_hoverThrottle;
gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
@ -214,11 +214,11 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
{ "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } }, { "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 } }, { "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){ &positionControlConfig_landingAltitudeM, 1, 15, 1 } }, { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_landingAltitudeM, 1, 15, 1 } },
{ "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } }, { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMin, 1050, 1400, 1 } },
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } }, { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMax, 1400, 2000, 1 } },
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &positionControlConfig_hoverThrottle, 1100, 1700, 1 } }, { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_hoverThrottle, 1100, 1700, 1 } },
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 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 }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },

View file

@ -95,6 +95,7 @@
#include "fc/tasks.h" #include "fc/tasks.h"
#include "flight/alt_hold.h" #include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -102,7 +103,6 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/pid_init.h" #include "flight/pid_init.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"
@ -828,7 +828,7 @@ void init(void)
#endif #endif
positionInit(); positionInit();
positionControlInit(positionControlConfig()); autopilotInit(autopilotConfig());
#if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL) #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
vtxTableInit(); vtxTableInit();
@ -1000,7 +1000,7 @@ void init(void)
spiInitBusDMA(); spiInitBusDMA();
#endif #endif
// position_control must be initialised before modes that require the position_control pids // autopilot must be initialised before modes that require the autopilot pids
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
altHoldInit(); altHoldInit();
#endif #endif

View file

@ -158,12 +158,15 @@
#define PARAM_NAME_ALTITUDE_PREFER_BARO "altitude_prefer_baro" #define PARAM_NAME_ALTITUDE_PREFER_BARO "altitude_prefer_baro"
#define PARAM_NAME_ALTITUDE_LPF "altitude_lpf" #define PARAM_NAME_ALTITUDE_LPF "altitude_lpf"
#define PARAM_NAME_ALTITUDE_D_LPF "altitude_d_lpf" #define PARAM_NAME_ALTITUDE_D_LPF "altitude_d_lpf"
#define PARAM_NAME_HOVER_THROTTLE "hover_throttle" #define PARAM_NAME_HOVER_THROTTLE "hover_throttle"
#define PARAM_NAME_LANDING_ALTITUDE "landing_altitude_m" #define PARAM_NAME_LANDING_ALTITUDE "landing_altitude_m"
#define PARAM_NAME_ALTITUDE_P "altitude_P" #define PARAM_NAME_THROTTLE_MIN "autopilot_throttle_min"
#define PARAM_NAME_ALTITUDE_I "altitude_I" #define PARAM_NAME_THROTTLE_MAX "autopilot_throttle_max"
#define PARAM_NAME_ALTITUDE_D "altitude_D" #define PARAM_NAME_ALTITUDE_P "autopilot_altitude_P"
#define PARAM_NAME_ALTITUDE_F "altitude_F" #define PARAM_NAME_ALTITUDE_I "autopilot_altitude_I"
#define PARAM_NAME_ALTITUDE_D "autopilot_altitude_D"
#define PARAM_NAME_ALTITUDE_F "autopilot_altitude_F"
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward" #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
#define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
@ -212,9 +215,6 @@
#define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate" #define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
#define PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD "gps_rescue_disarm_threshold" #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_SANITY_CHECKS "gps_rescue_sanity_checks" #define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks"
#define PARAM_NAME_GPS_RESCUE_MIN_SATS "gps_rescue_min_sats" #define PARAM_NAME_GPS_RESCUE_MIN_SATS "gps_rescue_min_sats"
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix" #define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
@ -240,8 +240,6 @@
#endif // USE_GPS #endif // USE_GPS
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
#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" #define PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE "alt_hold_target_adjust_rate"
#endif // USE_ALT_HOLD_MODE #endif // USE_ALT_HOLD_MODE

View file

@ -16,124 +16,52 @@
*/ */
#include "platform.h" #include "platform.h"
#include "math.h"
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
#include "math.h"
#include "build/debug.h" #include "build/debug.h"
#include "common/maths.h"
#include "config/config.h" #include "config/config.h"
#include "fc/runtime_config.h"
#include "fc/rc.h" #include "fc/rc.h"
#include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "sensors/acceleration.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "alt_hold.h" #include "alt_hold.h"
static float pidIntegral = 0.0f;
static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
altHoldState_t altHoldState; altHoldState_t altHoldState;
float altitudePidCalculate(void) void controlAltitude(void)
{ {
// * introductory notes *
// this is a classical 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 pidCoefficient_t *pidCoefs = getAltitudePidCoeffs();
const float altitudeErrorCm = altHoldState.targetAltitudeCm - getAltitudeCm();
// P
const float pOut = pidCoefs->Kp * altitudeErrorCm;
// 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
// much less iTerm change for errors greater than 2m, otherwise it winds up badly
const float itermNormalRange = 200.0f; // 2m
const float itermRelax = (fabsf(altitudeErrorCm) < itermNormalRange) ? 1.0f : 0.1f;
pidIntegral += altitudeErrorCm * pidCoefs->Ki * itermRelax * taskIntervalSeconds;
// arbitrary limit on iTerm, same as for gps_rescue, +/-20% of full throttle range
// ** might not be needed with input limiting **
pidIntegral = constrainf(pidIntegral, -200.0f, 200.0f);
const float iOut = pidIntegral;
// D
// boost D by 'increasing apparent velocity' when vertical velocity exceeds 5 m/s ( D of 75 on defaults) // boost D by 'increasing apparent velocity' when vertical velocity exceeds 5 m/s ( D of 75 on defaults)
// usually we don't see fast ascend/descend rates if the altitude hold starts under stable conditions // 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; // this is important primarily to arrest pre-existing fast drops or climbs at the start;
float verticalVelocity = getAltitudeDerivative(); // cm/s altitude derivative is always available
float vel = getAltitudeDerivative(); // cm/s altitude derivative is always available
const float kinkPoint = 500.0f; // velocity at which D should start to increase const float kinkPoint = 500.0f; // velocity at which D should start to increase
const float kinkPointAdjustment = kinkPoint * 2.0f; // Precompute constant const float kinkPointAdjustment = kinkPoint * 2.0f; // Precompute constant
const float sign = (vel > 0) ? 1.0f : -1.0f; const float sign = (verticalVelocity > 0) ? 1.0f : -1.0f;
if (fabsf(vel) > kinkPoint) { if (fabsf(verticalVelocity) > kinkPoint) {
vel = vel * 3.0f - sign * kinkPointAdjustment; verticalVelocity = verticalVelocity * 3.0f - sign * kinkPointAdjustment;
} }
const float dOut = pidCoefs->Kd * vel; //run the function in autopilot.c that calculates the PIDs and drives the motors
altitudeControl(altHoldState.targetAltitudeCm, taskIntervalSeconds, verticalVelocity, altHoldState.targetAltitudeAdjustRate);
// 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 = pidCoefs->Kf * altHoldState.targetAltitudeAdjustRate;
// to further improve performance...
// adding a high-pass filtered amount of FF would give a boost when altitude changes were requested
// this would offset motor lag and kick off some initial vertical acceleration.
// this would be exactly what an experienced pilot would do.
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 altHoldReset(void) void altHoldReset(void)
{ {
pidIntegral = 0.0f; resetAltitudeControl();
altHoldState.targetAltitudeCm = getAltitudeCm(); altHoldState.targetAltitudeCm = getAltitudeCm();
altHoldState.targetAltitudeAdjustRate = 0.0f; altHoldState.targetAltitudeAdjustRate = 0.0f;
} }
void altHoldInit(void) void altHoldInit(void)
{ {
altHoldState.hoverThrottle = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
altHoldState.isAltHoldActive = false; altHoldState.isAltHoldActive = false;
altHoldReset(); altHoldReset();
} }
@ -177,8 +105,8 @@ void altHoldUpdateTargetAltitude(void)
const float rcThrottle = rcCommand[THROTTLE]; const float rcThrottle = rcCommand[THROTTLE];
const float lowThreshold = 0.5f * (positionControlConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300 const float lowThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300
const float highThreshold = 0.5f * (positionControlConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300 const float highThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300
float throttleAdjustmentFactor = 0.0f; float throttleAdjustmentFactor = 0.0f;
if (rcThrottle < lowThreshold) { if (rcThrottle < lowThreshold) {
@ -186,7 +114,6 @@ void altHoldUpdateTargetAltitude(void)
} else if (rcThrottle > highThreshold) { } else if (rcThrottle > highThreshold) {
throttleAdjustmentFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f); 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 failsafe is active, and we get here, we are in failsafe landing mode
if (failsafeIsActive()) { if (failsafeIsActive()) {
// descend at up to 10 times faster when high // descend at up to 10 times faster when high
@ -198,8 +125,8 @@ void altHoldUpdateTargetAltitude(void)
// constant (set) deceleration target in the last 2m // constant (set) deceleration target in the last 2m
throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f)); throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
} }
altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate; 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 // 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; altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate * taskIntervalSeconds;
} }
@ -211,18 +138,7 @@ void altHoldUpdate(void)
altHoldUpdateTargetAltitude(); altHoldUpdateTargetAltitude();
} }
// use PIDs to return the throttle adjustment value, add it to the hover value, and constrain controlAltitude();
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.hoverThrottle + 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(newThrottle)); // normal range 1000-2000, but is beforeconstraint
DEBUG_SET(DEBUG_ALTHOLD, 3, lrintf(tiltMultiplier * 100));
} }
void updateAltHoldState(timeUs_t currentTimeUs) { void updateAltHoldState(timeUs_t currentTimeUs) {
@ -235,13 +151,4 @@ void updateAltHoldState(timeUs_t currentTimeUs) {
altHoldUpdate(); altHoldUpdate();
} }
} }
float altHoldGetThrottle(void) {
// see notes in gpsRescueGetThrottle() about mincheck
float commandedThrottle = scaleRangef(altHoldState.throttleOut, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
// with high values for mincheck, we could sclae to negative throttle values.
commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
return commandedThrottle;
}
#endif #endif

View file

@ -28,12 +28,9 @@ typedef struct {
bool isAltHoldActive; bool isAltHoldActive;
float targetAltitudeCm; float targetAltitudeCm;
float targetAltitudeAdjustRate; float targetAltitudeAdjustRate;
float throttleOut;
float hoverThrottle;
} altHoldState_t; } altHoldState_t;
void altHoldInit(void); void altHoldInit(void);
void updateAltHoldState(timeUs_t currentTimeUs); void updateAltHoldState(timeUs_t currentTimeUs);
float altHoldGetThrottle(void);
#endif #endif

107
src/main/flight/autopilot.c Normal file
View file

@ -0,0 +1,107 @@
/*
* 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 <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "rx/rx.h"
#include "autopilot.h"
#define ALTITUDE_P_SCALE 0.01f
#define ALTITUDE_I_SCALE 0.003f
#define ALTITUDE_D_SCALE 0.01f
#define ALTITUDE_F_SCALE 0.01f
static pidCoefficient_t altitudePidCoeffs;
static float altitudeI = 0.0f;
static float throttleOut = 0.0f;
void autopilotInit(const autopilotConfig_t *config)
{
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
altitudePidCoeffs.Kf = config->altitude_F * ALTITUDE_F_SCALE;
}
void resetAltitudeControl (void) {
altitudeI = 0.0f;
}
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep) {
const float altitudeErrorCm = targetAltitudeCm - getAltitudeCm();
const float altitudeP = altitudeErrorCm * altitudePidCoeffs.Kp;
// reduce the iTerm gain for errors greater than 200cm (2m), otherwise it winds up too much
const float itermRelax = (fabsf(altitudeErrorCm) < 200.0f) ? 1.0f : 0.1f;
altitudeI += altitudeErrorCm * altitudePidCoeffs.Ki * itermRelax * taskIntervalS;
// limit iTerm to not more than 200 throttle units
altitudeI = constrainf(altitudeI, -200.0f, 200.0f);
const float altitudeD = verticalVelocity * altitudePidCoeffs.Kd;
const float altitudeF = targetAltitudeStep * altitudePidCoeffs.Kf;
const float hoverOffset = autopilotConfig()->hover_throttle - PWM_RANGE_MIN;
float throttleOffset = altitudeP + altitudeI - altitudeD + altitudeF + hoverOffset;
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
throttleOffset *= tiltMultiplier;
float newThrottle = PWM_RANGE_MIN + throttleOffset;
newThrottle = constrainf(newThrottle, autopilotConfig()->throttle_min, autopilotConfig()->throttle_max);
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 0, lrintf(newThrottle)); // normal range 1000-2000 but is before constraint
newThrottle = scaleRangef(newThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
throttleOut = constrainf(newThrottle, 0.0f, 1.0f);
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 1, lrintf(tiltMultiplier * 100));
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 3, lrintf(targetAltitudeCm));
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 4, lrintf(altitudeP));
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 5, lrintf(altitudeI));
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 6, lrintf(-altitudeD));
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 7, lrintf(altitudeF));
}
bool isBelowLandingAltitude(void)
{
return getAltitudeCm() < 100.0f * autopilotConfig()->landing_altitude_m;
}
const pidCoefficient_t *getAltitudePidCoeffs(void)
{
return &altitudePidCoeffs;
}
float getAutopilotThrottle(void)
{
return throttleOut;
}

View file

@ -17,11 +17,14 @@
#pragma once #pragma once
#include "pg/position_control.h" #include "pg/autopilot.h"
#include "flight/pid.h" #include "flight/pid.h"
void positionControlInit(const positionControlConfig_t *config); void autopilotInit(const autopilotConfig_t *config);
void resetAltitudeControl(void);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
bool isBelowLandingAltitude(void); bool isBelowLandingAltitude(void);
const pidCoefficient_t *getAltitudePidCoeffs(void); const pidCoefficient_t *getAltitudePidCoeffs(void);
float getAutopilotThrottle(void);

View file

@ -30,24 +30,22 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
#include "config/config.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "io/gps.h"
#include "config/config.h"
#include "fc/core.h" #include "fc/core.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "io/gps.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "gps_rescue.h" #include "gps_rescue.h"
@ -208,8 +206,6 @@ static void rescueAttainPosition(void)
// runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase. // runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
static float previousVelocityError = 0.0f; static float previousVelocityError = 0.0f;
static float velocityI = 0.0f; static float velocityI = 0.0f;
static float throttleI = 0.0f;
switch (rescueState.phase) { switch (rescueState.phase) {
case RESCUE_IDLE: case RESCUE_IDLE:
// values to be returned when no rescue is active // values to be returned when no rescue is active
@ -221,7 +217,7 @@ static void rescueAttainPosition(void)
// Initialize internal variables each time GPS Rescue is started // Initialize internal variables each time GPS Rescue is started
previousVelocityError = 0.0f; previousVelocityError = 0.0f;
velocityI = 0.0f; velocityI = 0.0f;
throttleI = 0.0f; resetAltitudeControl();
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f; rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
rescueState.sensor.imuYawCogGain = 1.0f; rescueState.sensor.imuYawCogGain = 1.0f;
return; return;
@ -229,7 +225,7 @@ static void rescueAttainPosition(void)
// 20s of slow descent for switch induced sanity failures to allow time to recover // 20s of slow descent for switch induced sanity failures to allow time to recover
gpsRescueAngle[AI_PITCH] = 0.0f; gpsRescueAngle[AI_PITCH] = 0.0f;
gpsRescueAngle[AI_ROLL] = 0.0f; gpsRescueAngle[AI_ROLL] = 0.0f;
rescueThrottle = positionControlConfig()->hover_throttle - 100; rescueThrottle = autopilotConfig()->hover_throttle - 100;
return; return;
default: default:
break; break;
@ -238,48 +234,9 @@ static void rescueAttainPosition(void)
/** /**
Altitude (throttle) controller Altitude (throttle) controller
*/ */
const float altitudeErrorCm = (rescueState.intent.targetAltitudeCm - getAltitudeCm());
// height above target in cm (negative means too low)
// at the start, the target starts at current altitude plus one step. Increases stepwise to intended value.
// P component const float verticalVelocity = getAltitudeDerivative() * rescueState.intent.throttleDMultiplier;
const float throttleP = getAltitudePidCoeffs()->Kp * altitudeErrorCm; altitudeControl(rescueState.intent.targetAltitudeCm, taskIntervalSeconds, verticalVelocity, rescueState.intent.targetAltitudeStepCm);
// I component
// reduce the iTerm gain for errors greater than 2m, otherwise it winds up too much
const float itermNormalRange = 200.0f; // 2m
const float itermRelax = (fabsf(altitudeErrorCm) < itermNormalRange) ? 1.0f : 0.1f;
throttleI += altitudeErrorCm * getAltitudePidCoeffs()->Ki * itermRelax * taskIntervalSeconds;
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM, 1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM);
// up to 20% increase in throttle from I alone, need to check if this is needed, in practice.
// D component
const float throttleD = getAltitudeDerivative() * getAltitudePidCoeffs()->Kd * rescueState.intent.throttleDMultiplier;
// F component
// add a feedforward element that is proportional to the ascend or descend rate
const float throttleF = getAltitudePidCoeffs()->Kf * rescueState.intent.targetAltitudeStepCm * TASK_GPS_RESCUE_RATE_HZ;
const float hoverOffset = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
const float tiltMultiplier = 2.0f - fmaxf(getCosTiltAngle(), 0.5f); // same code as alt_hold
// 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
rescueThrottle = PWM_RANGE_MIN + (throttleP + throttleI - throttleD + throttleF + hoverOffset) * tiltMultiplier;
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, lrintf(rescueThrottle)); // normal range 1000-2000 but is before constraint
// constrain rescue throttle
rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, lrintf(tiltMultiplier * 100));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 4, lrintf(throttleP));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 5, lrintf(throttleI));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 6, lrintf(throttleD));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 7, lrintf(throttleF));
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 6, lrintf(rescueThrottle)); // throttle value to use during a rescue
/** /**
Heading / yaw controller Heading / yaw controller
@ -687,7 +644,7 @@ void descend(void)
{ {
if (newGPSData) { if (newGPSData) {
// consider landing area to be a circle half landing height around home, to avoid overshooting home point // consider landing area to be a circle half landing height around home, to avoid overshooting home point
const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * positionControlConfig()->landing_altitude_m); const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * autopilotConfig()->landing_altitude_m);
const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); 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 // increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home
@ -918,7 +875,6 @@ void gpsRescueUpdate(void)
} }
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, lrintf(rescueState.intent.targetAltitudeCm)); DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, lrintf(rescueState.intent.targetAltitudeCm));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, lrintf(rescueState.intent.targetAltitudeCm));
DEBUG_SET(DEBUG_RTH, 0, lrintf(rescueState.intent.maxAltitudeCm / 10.0f)); DEBUG_SET(DEBUG_RTH, 0, lrintf(rescueState.intent.maxAltitudeCm / 10.0f));
performSanityChecks(); performSanityChecks();
@ -937,20 +893,6 @@ float gpsRescueGetImuYawCogGain(void)
return rescueState.sensor.imuYawCogGain; return rescueState.sensor.imuYawCogGain;
} }
float gpsRescueGetThrottle(void)
{
// Calculate the commanded throttle for use in the mixer. Output must be within range 0.0 - 1.0.
// minCheck can be less than, or greater than, PWM_RANGE_MIN, but is usually at default of 1050
// it is the value at which the user expects the motors to start spinning (leaving a deadband from 1000 to 1050)
// rescue throttle min can't be less than gps_rescue_throttle_min (1100) or greater than max (1750)
// we scale throttle from mincheck to PWM_RANGE_MAX when mincheck is greater than PWM_RANGE_MIN, otherwise from PWM_RANGE_MIN to PWM_RANGE_MAX
float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
// if mincheck is set below PWRM_RANGE_MIN, the gps rescue throttle may seem greater than expected
// with high values for mincheck, we could sclae to negative throttle values.
commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
return commandedThrottle;
}
bool gpsRescueIsConfigured(void) bool gpsRescueIsConfigured(void)
{ {
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE); return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE);

View file

@ -51,7 +51,6 @@ void gpsRescueInit(void);
void gpsRescueUpdate(void); void gpsRescueUpdate(void);
void gpsRescueNewGpsData(void); void gpsRescueNewGpsData(void);
float gpsRescueGetYawRate(void); float gpsRescueGetYawRate(void);
float gpsRescueGetThrottle(void);
bool gpsRescueIsConfigured(void); bool gpsRescueIsConfigured(void);
bool gpsRescueIsAvailable(void); bool gpsRescueIsAvailable(void);
bool gpsRescueIsDisabled(void); bool gpsRescueIsDisabled(void);

View file

@ -45,8 +45,9 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/alt_hold.h" #include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer_init.h" #include "flight/mixer_init.h"
@ -801,7 +802,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
// Throttle value to be used during altitude hold mode (and failsafe landing mode) // Throttle value to be used during altitude hold mode (and failsafe landing mode)
if (FLIGHT_MODE(ALT_HOLD_MODE)) { if (FLIGHT_MODE(ALT_HOLD_MODE)) {
throttle = altHoldGetThrottle(); throttle = getAutopilotThrottle();
} }
#endif #endif
@ -809,7 +810,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
// If gps rescue is active then override the throttle. This prevents things // If gps rescue is active then override the throttle. This prevents things
// like throttle boost or throttle limit from negatively affecting the throttle. // like throttle boost or throttle limit from negatively affecting the throttle.
if (FLIGHT_MODE(GPS_RESCUE_MODE)) { if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
throttle = gpsRescueGetThrottle(); throttle = getAutopilotThrottle();
} }
#endif #endif

View file

@ -47,10 +47,10 @@
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/alt_hold.h" #include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/position_control.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "io/gps.h" #include "io/gps.h"

View file

@ -34,7 +34,6 @@
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/pid.h" #include "flight/pid.h"
@ -211,8 +210,7 @@ void calculateEstimatedAltitude(void)
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario); DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
#endif #endif
DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f)); DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
DEBUG_SET(DEBUG_ALTHOLD, 1, lrintf(zeroedAltitudeCm)); DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedAltitudeCm));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(zeroedAltitudeCm));
altitudeAvailable = haveGpsAlt || haveBaroAlt; altitudeAvailable = haveGpsAlt || haveBaroAlt;
} }

View file

@ -1,50 +0,0 @@
/*
* 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 <stdlib.h>
#include <stdbool.h>
#include "platform.h"
#include "position.h"
#include "position_control.h"
#define ALTITUDE_P_SCALE 0.01f
#define ALTITUDE_I_SCALE 0.003f
#define ALTITUDE_D_SCALE 0.01f
#define ALTITUDE_F_SCALE 0.01f
static pidCoefficient_t altitudePidCoeffs;
void positionControlInit(const positionControlConfig_t *config)
{
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
altitudePidCoeffs.Kf = config->altitude_F * ALTITUDE_F_SCALE;
}
bool isBelowLandingAltitude(void)
{
return getAltitudeCm() < 100.0f * positionControlConfig()->landing_altitude_m;
}
const pidCoefficient_t *getAltitudePidCoeffs(void)
{
return &altitudePidCoeffs;
}

View file

@ -82,6 +82,7 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
@ -89,7 +90,6 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/pid_init.h" #include "flight/pid_init.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/servos.h" #include "flight/servos.h"
@ -1544,9 +1544,9 @@ case MSP_NAME:
sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM); sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM);
sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM); sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS); sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS);
sbufWriteU16(dst, gpsRescueConfig()->throttleMin); sbufWriteU16(dst, autopilotConfig()->throttle_min);
sbufWriteU16(dst, gpsRescueConfig()->throttleMax); sbufWriteU16(dst, autopilotConfig()->throttle_max);
sbufWriteU16(dst, positionControlConfig()->hover_throttle); sbufWriteU16(dst, autopilotConfig()->hover_throttle);
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
sbufWriteU8(dst, gpsRescueConfig()->minSats); sbufWriteU8(dst, gpsRescueConfig()->minSats);
@ -1562,9 +1562,9 @@ case MSP_NAME:
break; break;
case MSP_GPS_RESCUE_PIDS: case MSP_GPS_RESCUE_PIDS:
sbufWriteU16(dst, positionControlConfig()->altitude_P); sbufWriteU16(dst, autopilotConfig()->altitude_P);
sbufWriteU16(dst, positionControlConfig()->altitude_I); sbufWriteU16(dst, autopilotConfig()->altitude_I);
sbufWriteU16(dst, positionControlConfig()->altitude_D); sbufWriteU16(dst, autopilotConfig()->altitude_D);
// altitude_F not implemented yet // altitude_F not implemented yet
sbufWriteU16(dst, gpsRescueConfig()->velP); sbufWriteU16(dst, gpsRescueConfig()->velP);
sbufWriteU16(dst, gpsRescueConfig()->velI); sbufWriteU16(dst, gpsRescueConfig()->velI);
@ -2879,9 +2879,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src); gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src);
gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src); gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src); gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src);
gpsRescueConfigMutable()->throttleMin = sbufReadU16(src); autopilotConfigMutable()->throttle_min = sbufReadU16(src);
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src); autopilotConfigMutable()->throttle_max = sbufReadU16(src);
positionControlConfigMutable()->hover_throttle = sbufReadU16(src); autopilotConfigMutable()->hover_throttle = sbufReadU16(src);
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
gpsRescueConfigMutable()->minSats = sbufReadU8(src); gpsRescueConfigMutable()->minSats = sbufReadU8(src);
if (sbufBytesRemaining(src) >= 6) { if (sbufBytesRemaining(src) >= 6) {
@ -2902,9 +2902,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
break; break;
case MSP_SET_GPS_RESCUE_PIDS: case MSP_SET_GPS_RESCUE_PIDS:
positionControlConfigMutable()->altitude_P = sbufReadU16(src); autopilotConfigMutable()->altitude_P = sbufReadU16(src);
positionControlConfigMutable()->altitude_I = sbufReadU16(src); autopilotConfigMutable()->altitude_I = sbufReadU16(src);
positionControlConfigMutable()->altitude_D = sbufReadU16(src); autopilotConfigMutable()->altitude_D = sbufReadU16(src);
// altitude_F not included in msp yet // altitude_F not included in msp yet
gpsRescueConfigMutable()->velP = sbufReadU16(src); gpsRescueConfigMutable()->velP = sbufReadU16(src);
gpsRescueConfigMutable()->velI = sbufReadU16(src); gpsRescueConfigMutable()->velI = sbufReadU16(src);

View file

@ -33,7 +33,5 @@ PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFI
PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig,
.alt_hold_target_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s .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 #endif

View file

@ -26,8 +26,6 @@
typedef struct altholdConfig_s { typedef struct altholdConfig_s {
uint8_t alt_hold_target_adjust_rate; uint8_t alt_hold_target_adjust_rate;
uint16_t alt_hold_throttle_min;
uint16_t alt_hold_throttle_max;
} altholdConfig_t; } altholdConfig_t;
PG_DECLARE(altholdConfig_t, altholdConfig); PG_DECLARE(altholdConfig_t, altholdConfig);

View file

@ -20,18 +20,20 @@
#include "platform.h" #include "platform.h"
#include "flight/position_control.h" #include "flight/autopilot.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "position_control.h" #include "autopilot.h"
PG_REGISTER_WITH_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0); PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 1);
PG_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig, PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
.hover_throttle = 1275,
.landing_altitude_m = 4, .landing_altitude_m = 4,
.hover_throttle = 1275,
.throttle_min = 1100,
.throttle_max = 1700,
.altitude_P = 15, .altitude_P = 15,
.altitude_I = 15, .altitude_I = 15,
.altitude_D = 15, .altitude_D = 15,

View file

@ -24,14 +24,16 @@
#include "pg/pg.h" #include "pg/pg.h"
typedef struct positionControlConfig_s { typedef struct autopilotConfig_s {
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 uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres
uint16_t hover_throttle; // value used at the start of a rescue or position hold
uint16_t throttle_min;
uint16_t throttle_max;
uint8_t altitude_P; uint8_t altitude_P;
uint8_t altitude_I; uint8_t altitude_I;
uint8_t altitude_D; uint8_t altitude_D;
uint8_t altitude_F; uint8_t altitude_F;
} positionControlConfig_t; } autopilotConfig_t;
PG_DECLARE(positionControlConfig_t, positionControlConfig); PG_DECLARE(autopilotConfig_t, autopilotConfig);

View file

@ -48,9 +48,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.descendRate = 150, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent .descendRate = 150, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
.disarmThreshold = 30, .disarmThreshold = 30,
.throttleMin = 1100,
.throttleMax = 1700,
.allowArmingWithoutFix = false, .allowArmingWithoutFix = false,
.sanityChecks = RESCUE_SANITY_FS_ONLY, .sanityChecks = RESCUE_SANITY_FS_ONLY,
.minSats = 8, .minSats = 8,

View file

@ -31,8 +31,6 @@ typedef struct gpsRescue_s {
uint16_t descentDistanceM; // meters uint16_t descentDistanceM; // meters
uint16_t groundSpeedCmS; // centimeters per second uint16_t groundSpeedCmS; // centimeters per second
uint8_t yawP; uint8_t yawP;
uint16_t throttleMin;
uint16_t throttleMax;
uint8_t minSats; uint8_t minSats;
uint8_t velP, velI, velD; uint8_t velP, velI, velD;
uint16_t minStartDistM; // meters uint16_t minStartDistM; // meters

View file

@ -83,7 +83,7 @@
#define PG_VTX_IO_CONFIG 57 #define PG_VTX_IO_CONFIG 57
#define PG_GPS_LAP_TIMER 58 #define PG_GPS_LAP_TIMER 58
#define PG_ALTHOLD_CONFIG 59 #define PG_ALTHOLD_CONFIG 59
#define PG_POSITION_CONTROL 60 #define PG_AUTOPILOT 60
// Driver configuration // Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight

View file

@ -709,7 +709,7 @@ void detectAndApplySignalLossBehaviour(void)
if (!thisChannelValid) { if (!thisChannelValid) {
if (channel == THROTTLE ) { if (channel == THROTTLE ) {
sample = failsafeConfig()->failsafe_throttle; sample = failsafeConfig()->failsafe_throttle;
// stage 2 failsafe throttle value. In GPS Rescue Flight mode, gpsRescueGetThrottle overrides, late in mixer.c // stage 2 failsafe throttle value. In GPS Rescue Flight mode, altitude control overrides, late in mixer.c
} else { } else {
sample = rxConfig()->midrc; sample = rxConfig()->midrc;
} }

View file

@ -43,8 +43,10 @@ arming_prevention_unittest_SRC := \
$(USER_DIR)/fc/rc_controls.c \ $(USER_DIR)/fc/rc_controls.c \
$(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/autopilot.c \
$(USER_DIR)/flight/gps_rescue.c $(USER_DIR)/flight/gps_rescue.c
arming_prevention_unittest_DEFINES := \ arming_prevention_unittest_DEFINES := \
USE_GPS_RESCUE= USE_GPS_RESCUE=
@ -466,6 +468,7 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \
althold_unittest_SRC := \ althold_unittest_SRC := \
$(USER_DIR)/flight/alt_hold.c \ $(USER_DIR)/flight/alt_hold.c \
$(USER_DIR)/flight/autopilot.c \
$(USER_DIR)/common/maths.c \ $(USER_DIR)/common/maths.c \
$(USER_DIR)/pg/rx.c $(USER_DIR)/pg/rx.c

View file

@ -31,11 +31,11 @@ extern "C" {
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/alt_hold.h" #include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/position.h"
#include "flight/position_control.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -43,12 +43,10 @@ extern "C" {
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0); PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0); PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
extern altHoldState_t altHoldState; extern altHoldState_t altHoldState;
void altHoldReset(void);
void altHoldProcessTransitions(void);
void altHoldInit(void); void altHoldInit(void);
void updateAltHoldState(timeUs_t); void updateAltHoldState(timeUs_t);
bool failsafeIsActive(void) { return false; } bool failsafeIsActive(void) { return false; }
@ -100,33 +98,11 @@ TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter)
extern "C" { extern "C" {
acc_t acc; acc_t acc;
pidCoefficient_t testAltitudePidCoeffs = {15.0f, 15.0f, 15.1f, 15.0f}; float getAltitudeCm(void) {return 0.0f;}
const pidCoefficient_t *getAltitudePidCoeffs(void) { float getAltitudeDerivative(void) {return 0.0f;}
return &testAltitudePidCoeffs;
}
float getAltitudeCm(void) { return 0.0f;}
float getAltitudeDerivative(void) { return 0.0f;}
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 getCosTiltAngle(void) { return 0.0f; } float getCosTiltAngle(void) { return 0.0f; }
float rcCommand[4]; float rcCommand[4];
float getRcDeflection(int)
{
return 0;
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig) void parseRcChannels(const char *input, rxConfig_t *rxConfig)
{ {
UNUSED(input); UNUSED(input);

View file

@ -1,196 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include <limits.h>
//#define DEBUG_ALTITUDE_HOLD
#define USE_BARO
extern "C" {
#include "debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/position.h"
#include "config/runtime_config.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DOWNWARDS_THRUST true
#define UPWARDS_THRUST false
extern "C" {
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
}
typedef struct inclinationExpectation_s {
rollAndPitchInclination_t inclination;
bool expectDownwardsThrust;
} inclinationExpectation_t;
TEST(AltitudeHoldTest, IsThrustFacingDownwards)
{
// given
inclinationExpectation_t inclinationExpectations[] = {
{ {{ 0, 0 }}, DOWNWARDS_THRUST },
{ {{ 799, 799 }}, DOWNWARDS_THRUST },
{ {{ 800, 799 }}, UPWARDS_THRUST },
{ {{ 799, 800 }}, UPWARDS_THRUST },
{ {{ 800, 800 }}, UPWARDS_THRUST },
{ {{ 801, 801 }}, UPWARDS_THRUST },
{ {{ -799, -799 }}, DOWNWARDS_THRUST },
{ {{ -800, -799 }}, UPWARDS_THRUST },
{ {{ -799, -800 }}, UPWARDS_THRUST },
{ {{ -800, -800 }}, UPWARDS_THRUST },
{ {{ -801, -801 }}, UPWARDS_THRUST }
};
uint8_t testIterationCount = sizeof(inclinationExpectations) / sizeof(inclinationExpectation_t);
// expect
for (uint8_t index = 0; index < testIterationCount; index ++) {
inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
#ifdef DEBUG_ALTITUDE_HOLD
printf("iteration: %d\n", index);
#endif
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
}
}
typedef struct inclinationAngleExpectations_s {
rollAndPitchInclination_t inclination;
uint16_t expected_angle;
} inclinationAngleExpectations_t;
TEST(AltitudeHoldTest, TestCalculateTiltAngle)
{
inclinationAngleExpectations_t inclinationAngleExpectations[] = {
{ {{ 0, 0}}, 0},
{ {{ 1, 0}}, 1},
{ {{ 0, 1}}, 1},
{ {{ 0, -1}}, 1},
{ {{-1, 0}}, 1},
{ {{-1, -2}}, 2},
{ {{-2, -1}}, 2},
{ {{ 1, 2}}, 2},
{ {{ 2, 1}}, 2}
};
rollAndPitchInclination_t inclination = {{0, 0}};
uint16_t tilt_angle = calculateTiltAngle(&inclination);
EXPECT_EQ(tilt_angle, 0);
for (uint8_t i = 0; i < 9; i++) {
inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
uint16_t result = calculateTiltAngle(&expectation->inclination);
EXPECT_EQ(expectation->expected_angle, result);
}
}
// STUBS
extern "C" {
uint32_t rcModeActivationMask;
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t accTimeSum ; // keep track for integration of acc
int accSumCount;
float accVelScale;
rollAndPitchInclination_t inclination;
//uint16_t acc_1G;
//int16_t heading;
//gyro_t gyro;
int32_t accSum[XYZ_AXIS_COUNT];
//int16_t magADC[XYZ_AXIS_COUNT];
int32_t BaroAlt;
int16_t debug[DEBUG16_VALUE_COUNT];
uint8_t stateFlags;
uint16_t flightModeFlags;
uint8_t armingFlags;
int32_t sonarAlt;
uint16_t enableFlightMode(flightModeFlags_e mask)
{
return flightModeFlags |= (mask);
}
uint16_t disableFlightMode(flightModeFlags_e mask)
{
return flightModeFlags &= ~(mask);
}
void gyroUpdate(void) {};
bool sensors(uint32_t mask)
{
UNUSED(mask);
return false;
};
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
UNUSED(rollAndPitchTrims);
}
void imuResetAccelerationSum(void) {};
int32_t applyDeadband(int32_t, int32_t) { return 0; }
uint32_t micros(void) { return 0; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; }
int constrain(int amt, int low, int high)
{
UNUSED(amt);
UNUSED(low);
UNUSED(high);
return 0;
}
}

View file

@ -36,13 +36,13 @@ extern "C" {
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "io/beeper.h" #include "io/beeper.h"
@ -78,7 +78,7 @@ extern "C" {
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0); PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t averageSystemLoadPercent = 0; uint16_t averageSystemLoadPercent = 0;
@ -1137,10 +1137,6 @@ extern "C" {
void pinioBoxTaskControl(void) {} void pinioBoxTaskControl(void) {}
void schedulerSetNextStateTime(timeDelta_t) {} void schedulerSetNextStateTime(timeDelta_t) {}
pidCoefficient_t testAltitudePidCoeffs = {15.0f, 15.0f, 15.1f, 15.0f};
const pidCoefficient_t *getAltitudePidCoeffs(void) {
return &testAltitudePidCoeffs;
}
float getAltitudeCm(void) {return 0.0f;} float getAltitudeCm(void) {return 0.0f;}
float getAltitudeDerivative(void) {return 0.0f;} float getAltitudeDerivative(void) {return 0.0f;}
@ -1164,5 +1160,4 @@ extern "C" {
void getRcDeflectionAbs(void) {} void getRcDeflectionAbs(void) {}
uint32_t getCpuPercentageLate(void) { return 0; } uint32_t getCpuPercentageLate(void) { return 0; }
bool crashFlipSuccessful(void) { return false; } bool crashFlipSuccessful(void) { return false; }
bool isBelowLandingAltitude(void) { return false; };
} }

View file

@ -42,11 +42,11 @@ extern "C" {
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/rc.h" #include "fc/rc.h"
#include "flight/autopilot.h"
#include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/position_control.h"
#include "io/gps.h" #include "io/gps.h"
@ -74,7 +74,7 @@ extern "C" {
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0); PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
PG_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = 0 .enabledFeatures = 0