diff --git a/mk/source.mk b/mk/source.mk
index 02e551bf3b..b4fe31814f 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -21,6 +21,7 @@ PG_SRC = \
pg/msp.c \
pg/pg.c \
pg/piniobox.c \
+ pg/position_control.c \
pg/pinio.c \
pg/pin_pull_up_down.c \
pg/rcdevice.c \
@@ -156,6 +157,7 @@ COMMON_SRC = \
fc/rc_controls.c \
fc/rc_modes.c \
flight/position.c \
+ flight/position_control.c \
flight/failsafe.c \
flight/gps_rescue.c \
fc/gps_lap_timer.c \
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 045d0914da..01f4e2e5a3 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -68,6 +68,7 @@
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
@@ -1588,8 +1589,12 @@ 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_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);
+ 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_ALTITUDE_P, "%d", positionControlConfig()->altitude_P);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", positionControlConfig()->altitude_I);;
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", positionControlConfig()->altitude_D);;
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", positionControlConfig()->altitude_F);
#ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
@@ -1688,9 +1693,6 @@ static bool blackboxWriteSysinfo(void)
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_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
- BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI)
- BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_D, "%d", gpsRescueConfig()->throttleD)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
@@ -1703,9 +1705,6 @@ static bool blackboxWriteSysinfo(void)
#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);
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 3bb8b08a6c..6073876b04 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -109,6 +109,7 @@ bool cliMode = false;
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index e6c53e7896..146395aa02 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -62,6 +62,7 @@
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/alt_hold.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
@@ -1066,9 +1067,6 @@ const clivalue_t valueTable[] = {
{ 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_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
- { PARAM_NAME_GPS_RESCUE_THROTTLE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },
- { PARAM_NAME_GPS_RESCUE_THROTTLE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) },
{ PARAM_NAME_GPS_RESCUE_VELOCITY_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) },
{ PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
{ PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
@@ -1093,10 +1091,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) },
#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) },
@@ -1823,8 +1817,14 @@ const clivalue_t valueTable[] = {
{ 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_POSITION_CONTROL
+ { 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_POSITION_CONTROL, offsetof(positionControlConfig_t, landing_altitude_m) },
+ { PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_P) },
+ { PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_I) },
+ { PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_D) },
+ { PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSITION_CONTROL, offsetof(positionControlConfig_t, altitude_F) },
// PG_MODE_ACTIVATION_CONFIG
#if defined(USE_CUSTOM_BOX_NAMES)
diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c
index 5be93f84a4..d959f9d6f9 100644
--- a/src/main/cms/cms_menu_gps_rescue.c
+++ b/src/main/cms/cms_menu_gps_rescue.c
@@ -39,6 +39,7 @@
#include "flight/gps_rescue.h"
#include "flight/position.h"
+#include "flight/position_control.h"
static uint16_t gpsRescueConfig_minStartDistM; //meters
static uint8_t gpsRescueConfig_altitudeMode;
@@ -51,16 +52,16 @@ static uint8_t gpsRescueConfig_angle; //degrees
static uint16_t gpsRescueConfig_descentDistanceM; //meters
static uint16_t gpsRescueConfig_descendRate;
-static uint8_t positionConfig_landingAltitudeM;
+static uint8_t positionControlConfig_landingAltitudeM;
static uint16_t gpsRescueConfig_throttleMin;
static uint16_t gpsRescueConfig_throttleMax;
-static uint16_t positionConfig_hoverThrottle;
+static uint16_t positionControlConfig_hoverThrottle;
static uint8_t gpsRescueConfig_minSats;
static uint8_t gpsRescueConfig_allowArmingWithoutFix;
-static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
+static uint8_t positionControlConfig_altitude_P, positionControlConfig_altitude_I, positionControlConfig_altitude_D, positionControlConfig_altitude_F;
static uint8_t gpsRescueConfig_yawP;
static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
@@ -71,9 +72,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);
- gpsRescueConfig_throttleP = gpsRescueConfig()->throttleP;
- gpsRescueConfig_throttleI = gpsRescueConfig()->throttleI;
- gpsRescueConfig_throttleD = gpsRescueConfig()->throttleD;
+ positionControlConfig_altitude_P = positionControlConfig()->altitude_P;
+ positionControlConfig_altitude_I = positionControlConfig()->altitude_I;
+ positionControlConfig_altitude_D = positionControlConfig()->altitude_D;
+ positionControlConfig_altitude_F = positionControlConfig()->altitude_F;
gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
@@ -92,9 +94,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
UNUSED(pDisp);
UNUSED(self);
- gpsRescueConfigMutable()->throttleP = gpsRescueConfig_throttleP;
- gpsRescueConfigMutable()->throttleI = gpsRescueConfig_throttleI;
- gpsRescueConfigMutable()->throttleD = gpsRescueConfig_throttleD;
+ positionControlConfigMutable()->altitude_P = positionControlConfig_altitude_P;
+ positionControlConfigMutable()->altitude_I = positionControlConfig_altitude_I;
+ positionControlConfigMutable()->altitude_D = positionControlConfig_altitude_D;
+ positionControlConfigMutable()->altitude_F = positionControlConfig_altitude_F;
gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
@@ -112,9 +115,10 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
{
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
- { "THROTTLE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleP, 0, 255, 1 } },
- { "THROTTLE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleI, 0, 255, 1 } },
- { "THROTTLE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleD, 0, 255, 1 } },
+ { "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_P, 0, 255, 1 } },
+ { "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_I, 0, 255, 1 } },
+ { "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_D, 0, 255, 1 } },
+ { "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_F, 0, 255, 1 } },
{ "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 255, 1 } },
@@ -155,11 +159,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
- positionConfig_landingAltitudeM = positionConfig()->landing_altitude_m;
+ positionControlConfig_landingAltitudeM = positionControlConfig()->landing_altitude_m;
gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin;
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
- positionConfig_hoverThrottle = positionConfig()->hover_throttle;
+ positionControlConfig_hoverThrottle = positionControlConfig()->hover_throttle;
gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
@@ -183,11 +187,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
- positionConfigMutable()->landing_altitude_m = positionConfig_landingAltitudeM;
+ positionControlConfigMutable()->landing_altitude_m = positionControlConfig_landingAltitudeM;
gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin;
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
- positionConfigMutable()->hover_throttle = positionConfig_hoverThrottle;
+ positionControlConfigMutable()->hover_throttle = positionControlConfig_hoverThrottle;
gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
@@ -210,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 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){ &positionConfig_landingAltitudeM, 1, 15, 1 } },
+ { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_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){ &positionConfig_hoverThrottle, 1100, 1700, 1 } },
+ { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &positionControlConfig_hoverThrottle, 1100, 1700, 1 } },
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } },
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 32ebaad959..2b340e066b 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -102,6 +102,7 @@
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"
@@ -762,9 +763,6 @@ void init(void)
#ifdef USE_GPS
if (featureIsEnabled(FEATURE_GPS)) {
gpsInit();
-#ifdef USE_GPS_RESCUE
- gpsRescueInit();
-#endif
#ifdef USE_GPS_LAP_TIMER
gpsLapTimerInit();
#endif // USE_GPS_LAP_TIMER
@@ -828,7 +826,9 @@ void init(void)
#ifdef USE_BARO
baroStartCalibration();
#endif
+
positionInit();
+ positionControlInit(positionControlConfig());
#if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
vtxTableInit();
@@ -1000,10 +1000,17 @@ void init(void)
spiInitBusDMA();
#endif
+// position_control must be initialised before modes that require the position_control pids
#ifdef USE_ALT_HOLD_MODE
altHoldInit();
#endif
+#ifdef USE_GPS_RESCUE
+ if (featureIsEnabled(FEATURE_GPS)) {
+ gpsRescueInit();
+ }
+#endif
+
debugInit();
unusedPinsInit();
diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h
index c8629e4f0c..5a054bba9b 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -147,12 +147,18 @@
#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_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_ALTITUDE_P "altitude_P"
+#define PARAM_NAME_ALTITUDE_I "altitude_I"
+#define PARAM_NAME_ALTITUDE_D "altitude_D"
+#define PARAM_NAME_ALTITUDE_F "altitude_F"
+
#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"
@@ -207,9 +213,6 @@
#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_THROTTLE_P "gps_rescue_throttle_p"
-#define PARAM_NAME_GPS_RESCUE_THROTTLE_I "gps_rescue_throttle_i"
-#define PARAM_NAME_GPS_RESCUE_THROTTLE_D "gps_rescue_throttle_d"
#define PARAM_NAME_GPS_RESCUE_VELOCITY_P "gps_rescue_velocity_p"
#define PARAM_NAME_GPS_RESCUE_VELOCITY_I "gps_rescue_velocity_i"
#define PARAM_NAME_GPS_RESCUE_VELOCITY_D "gps_rescue_velocity_d"
@@ -231,9 +234,6 @@
#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"
diff --git a/src/main/flight/alt_hold.c b/src/main/flight/alt_hold.c
index ab67b0afd2..a2ab94f26d 100644
--- a/src/main/flight/alt_hold.c
+++ b/src/main/flight/alt_hold.c
@@ -27,36 +27,23 @@
#include "fc/rc.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
+#include "flight/pid.h"
#include "flight/position.h"
+#include "flight/position_control.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;
+static float pidIntegral = 0.0f;
+static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
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
+ // 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:
@@ -80,31 +67,32 @@ float altitudePidCalculate(void)
// 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;
+ const pidCoefficient_t *pidCoefs = getAltitudePidCoeffs();
+
+ const float altitudeErrorCm = altHoldState.targetAltitudeCm - getAltitudeCm();
// P
- const float pOut = simplePid.kp * altErrorCm;
+ 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
- // no iTerm change for error greater than 2m, otherwise it winds up badly
+ // much less iTerm change for errors 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;
+ 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 **
- simplePid.integral = constrainf(simplePid.integral, -200.0f, 200.0f);
- const float iOut = simplePid.integral;
+ 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)
- // 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;
+ 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 kinkPointAdjustment = kinkPoint * 2.0f; // Precompute constant
const float sign = (vel > 0) ? 1.0f : -1.0f;
@@ -112,7 +100,7 @@ float altitudePidCalculate(void)
vel = vel * 3.0f - sign * kinkPointAdjustment;
}
- const float dOut = simplePid.kd * vel;
+ const float dOut = pidCoefs->Kd * vel;
// F
// if error is used, we get a 'free kick' in derivative from changes in the target value
@@ -120,9 +108,14 @@ float altitudePidCalculate(void)
// 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 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;
+ 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));
@@ -131,43 +124,16 @@ float altitudePidCalculate(void)
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;
+ pidIntegral = 0.0f;
+ altHoldState.targetAltitudeCm = getAltitudeCm();
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.hoverThrottle = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
altHoldState.isAltHoldActive = false;
altHoldReset();
}
@@ -211,8 +177,8 @@ void altHoldUpdateTargetAltitude(void)
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
+ 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 highThreshold = 0.5f * (positionControlConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300
float throttleAdjustmentFactor = 0.0f;
if (rcThrottle < lowThreshold) {
@@ -230,7 +196,7 @@ void altHoldUpdateTargetAltitude(void)
// 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));
+ throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
}
altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate;
@@ -251,28 +217,18 @@ void altHoldUpdate(void)
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;
+ 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(throttleAdjustment));
+ 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) {
- 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));
+ UNUSED(currentTimeUs);
+ // check for enabling Alt Hold, otherwise do as little as possible while inactive
altHoldProcessTransitions();
if (altHoldState.isAltHoldActive) {
@@ -281,7 +237,11 @@ void updateAltHoldState(timeUs_t currentTimeUs) {
}
float altHoldGetThrottle(void) {
- return scaleRangef(altHoldState.throttleOut, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
+ // 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
diff --git a/src/main/flight/alt_hold.h b/src/main/flight/alt_hold.h
index c7ccf32265..71280da23f 100644
--- a/src/main/flight/alt_hold.h
+++ b/src/main/flight/alt_hold.h
@@ -28,10 +28,8 @@ typedef struct {
bool isAltHoldActive;
float targetAltitudeCm;
float targetAltitudeAdjustRate;
- float measuredAltitudeCm;
float throttleOut;
- float hover;
- float smoothedVerticalVelocity;
+ float hoverThrottle;
} altHoldState_t;
void altHoldInit(void);
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 74ca83ab3d..821af1f03b 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -44,6 +44,7 @@
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "rx/rx.h"
@@ -79,6 +80,7 @@ typedef struct {
float maxAltitudeCm;
float returnAltitudeCm;
float targetAltitudeCm;
+ float targetAltitudeStepCm;
float targetVelocityCmS;
float pitchAngleLimitDeg;
float rollAngleLimitDeg;
@@ -95,7 +97,6 @@ typedef struct {
} rescueIntent_s;
typedef struct {
- float currentAltitudeCm;
float distanceToHomeCm;
float distanceToHomeM;
uint16_t groundSpeedCmS;
@@ -103,8 +104,6 @@ typedef struct {
bool healthy;
float errorAngle;
float gpsDataIntervalSeconds;
- float altitudeDataIntervalSeconds;
- float gpsRescueTaskIntervalSeconds;
float velocityToHomeCmS;
float alitutudeStepCm;
float maxPitchStep;
@@ -124,12 +123,12 @@ typedef struct {
#define GPS_RESCUE_MAX_THROTTLE_ITERM 200 // max iterm value for throttle in degrees * 100
#define GPS_RESCUE_ALLOWED_YAW_RANGE 30.0f // yaw error must be less than this to enter fly home phase, and to pitch during descend()
+static const float taskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); // i.e. 0.01 s
static float rescueThrottle;
static float rescueYaw;
-float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
-bool magForceDisable = false;
+float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
+bool magForceDisable = false;
static bool newGPSData = false;
-static pt2Filter_t throttleDLpf;
static pt1Filter_t velocityDLpf;
static pt3Filter_t velocityUpsampleLpf;
@@ -137,13 +136,7 @@ rescueState_s rescueState;
void gpsRescueInit(void)
{
- rescueState.sensor.gpsRescueTaskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
-
float cutoffHz, gain;
- cutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
- gain = pt2FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
- pt2FilterInit(&throttleDLpf, gain);
-
cutoffHz = gpsRescueConfig()->pitchCutoffHz / 100.0f;
rescueState.intent.velocityPidCutoff = cutoffHz;
rescueState.intent.velocityPidCutoffModifier = 1.0f;
@@ -151,7 +144,7 @@ void gpsRescueInit(void)
pt1FilterInit(&velocityDLpf, gain);
cutoffHz *= 4.0f;
- gain = pt3FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
+ gain = pt3FilterGain(cutoffHz, taskIntervalSeconds);
pt3FilterInit(&velocityUpsampleLpf, gain);
}
@@ -183,11 +176,11 @@ static void setReturnAltitude(void)
}
// While armed, but not during the rescue, update the max altitude value
- rescueState.intent.maxAltitudeCm = fmaxf(rescueState.sensor.currentAltitudeCm, rescueState.intent.maxAltitudeCm);
+ rescueState.intent.maxAltitudeCm = fmaxf(getAltitudeCm(), rescueState.intent.maxAltitudeCm);
if (newGPSData) {
- // set the target altitude to current values, so there will be no D kick on first run
- rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm;
+ // set the target altitude to the current altitude.
+ rescueState.intent.targetAltitudeCm = getAltitudeCm();
// Intended descent distance for rescues that start outside the minStartDistM distance
// Set this to the user's intended descent distance, but not more than half the distance to home to ensure some fly home time
@@ -200,7 +193,7 @@ static void setReturnAltitude(void)
break;
case GPS_RESCUE_ALT_MODE_CURRENT:
// climb above current altitude, but always return at least initial height above takeoff point, in case current altitude was negative
- rescueState.intent.returnAltitudeCm = fmaxf(initialClimbCm, rescueState.sensor.currentAltitudeCm + initialClimbCm);
+ rescueState.intent.returnAltitudeCm = fmaxf(initialClimbCm, getAltitudeCm() + initialClimbCm);
break;
case GPS_RESCUE_ALT_MODE_MAX:
default:
@@ -216,8 +209,6 @@ static void rescueAttainPosition(void)
static float previousVelocityError = 0.0f;
static float velocityI = 0.0f;
static float throttleI = 0.0f;
- static float previousAltitudeError = 0.0f;
- static int16_t throttleAdjustment = 0;
switch (rescueState.phase) {
case RESCUE_IDLE:
@@ -231,8 +222,6 @@ static void rescueAttainPosition(void)
previousVelocityError = 0.0f;
velocityI = 0.0f;
throttleI = 0.0f;
- previousAltitudeError = 0.0f;
- throttleAdjustment = 0;
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
rescueState.sensor.imuYawCogGain = 1.0f;
return;
@@ -240,7 +229,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 = positionConfig()->hover_throttle - 100;
+ rescueThrottle = positionControlConfig()->hover_throttle - 100;
return;
default:
break;
@@ -249,47 +238,47 @@ static void rescueAttainPosition(void)
/**
Altitude (throttle) controller
*/
- // currentAltitudeCm is updated at TASK_GPS_RESCUE_RATE_HZ
- const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100.0f;
- // height above target in metres (negative means too low)
+ 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 throttleP = gpsRescueConfig()->throttleP * altitudeError;
+ const float throttleP = getAltitudePidCoeffs()->Kp * altitudeErrorCm;
// I component
- throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
+ // 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
+ // up to 20% increase in throttle from I alone, need to check if this is needed, in practice.
- // D component is error based, so includes positive boost when climbing and negative boost on descent
- float throttleD = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
- previousAltitudeError = altitudeError;
- // increase by up to 2x when descent rate is faster
- throttleD *= rescueState.intent.throttleDMultiplier;
- // apply user's throttle D gain
- throttleD *= gpsRescueConfig()->throttleD;
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 6, lrintf(throttleD)); // throttle D before lowpass smoothing
- // smooth
- throttleD = pt2FilterApply(&throttleDLpf, throttleD);
+ // D component
+ const float throttleD = getAltitudeDerivative() * getAltitudePidCoeffs()->Kd * rescueState.intent.throttleDMultiplier;
- // acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now.
+ // 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;
- float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
- 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
+ const float hoverOffset = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
- throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
+ 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 = positionConfig()->hover_throttle + throttleAdjustment;
+ 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, 0, lrintf(throttleP));
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(throttleD));
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 4, lrintf(throttleI));
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 5, lrintf(tiltAdjustment)); // factor that adjusts throttle based on tilt angle
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 7, lrintf(throttleAdjustment)); // pidSum; amount to add/subtract from hover throttle value
+ 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
/**
@@ -415,7 +404,7 @@ static void performSanityChecks(void)
} else if (rescueState.phase == RESCUE_INITIALIZE) {
// Initialize these variables each time a GPS Rescue is started
previousTimeUs = currentTimeUs;
- prevAltitudeCm = rescueState.sensor.currentAltitudeCm;
+ prevAltitudeCm = getAltitudeCm();
prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm;
previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
secondsLowSats = 0;
@@ -501,10 +490,11 @@ static void performSanityChecks(void)
// These conditions ignore sanity mode settings, and apply in all rescues, to handle getting stuck in a climb or descend
- const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm;
+ const float actualAltitudeChange = getAltitudeCm() - prevAltitudeCm;
+ // ** possibly could use getAltitudeDerivative() for for actual altitude change, though it is smoothed
const float targetAltitudeChange = rescueState.intent.targetAltitudeCm - prevTargetAltitudeCm;
const float ratio = actualAltitudeChange / targetAltitudeChange;
- prevAltitudeCm = rescueState.sensor.currentAltitudeCm;
+ prevAltitudeCm = getAltitudeCm();
prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm;
switch (rescueState.phase) {
@@ -545,17 +535,9 @@ static void performSanityChecks(void)
static void sensorUpdate(void)
{
static float prevDistanceToHomeCm = 0.0f;
- const timeUs_t currentTimeUs = micros();
- static timeUs_t previousAltitudeDataTimeUs = 0;
- const timeDelta_t altitudeDataIntervalUs = cmpTimeUs(currentTimeUs, previousAltitudeDataTimeUs);
- rescueState.sensor.altitudeDataIntervalSeconds = altitudeDataIntervalUs * 0.000001f;
- previousAltitudeDataTimeUs = currentTimeUs;
-
- rescueState.sensor.currentAltitudeCm = getAltitude();
-
- DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, lrintf(rescueState.sensor.currentAltitudeCm));
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, lrintf(rescueState.sensor.currentAltitudeCm));
+ const float altitudeCurrentCm = getAltitudeCm();
+ DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, lrintf(altitudeCurrentCm));
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 0, rescueState.sensor.groundSpeedCmS); // groundspeed cm/s
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 1, gpsSol.groundCourse); // degrees * 10
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10
@@ -705,7 +687,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 - (0.5f * positionConfig()->landing_altitude_m);
+ const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * positionControlConfig()->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
@@ -738,23 +720,24 @@ void descend(void)
rescueState.intent.yawAttenuator = 1.0f;
// set the altitude step, considering the interval between altitude readings and the descent rate
- float altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+ float altitudeStepCm = taskIntervalSeconds * gpsRescueConfig()->descendRate;
// descend more slowly if the return home altitude was less than 20m
const float descentRateAttenuator = constrainf (rescueState.intent.returnAltitudeCm / 2000.0f, 0.25f, 1.0f);
- altitudeStep *= descentRateAttenuator;
+ altitudeStepCm *= descentRateAttenuator;
// slowest descent rate will be 1/4 of normal when we start descending at or below 5m above take-off point.
// otherwise a rescue initiated very low and close may not get all the way home
// descend faster while the quad is at higher altitudes
const float descentRateMultiplier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f);
- altitudeStep *= 1.0f + (2.0f * descentRateMultiplier);
+ altitudeStepCm *= 1.0f + (2.0f * descentRateMultiplier);
// maximum descent rate increase is 3x default above 50m, 2x above 25m, 1.2x at 5m, default by ground level
// also increase throttle D up to 2x in the descent phase when altitude descent rate is faster, for better control
rescueState.intent.throttleDMultiplier = 1.0f + descentRateMultiplier;
- rescueState.intent.targetAltitudeCm -= altitudeStep;
+ rescueState.intent.targetAltitudeStepCm = -altitudeStepCm;
+ rescueState.intent.targetAltitudeCm += rescueState.intent.targetAltitudeStepCm;
}
void initialiseRescueValues (void)
@@ -768,6 +751,7 @@ void initialiseRescueValues (void)
rescueState.intent.pitchAngleLimitDeg = 0.0f; // force pitch adjustment to zero - level mode will level out
rescueState.intent.velocityItermAttenuator = 1.0f; // allow iTerm to accumulate normally unless constrained by IMU error or descent phase
rescueState.intent.velocityItermRelax = 0.0f; // but don't accumulate any at the start, not until fly home
+ rescueState.intent.targetAltitudeStepCm = 0.0f;
}
void gpsRescueUpdate(void)
@@ -805,21 +789,21 @@ void gpsRescueUpdate(void)
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 && isAltitudeLow()) {
+ if (rescueState.sensor.distanceToHomeM < 5.0f && isBelowLandingAltitude()) {
// attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons
rescueState.phase = RESCUE_ABORT;
} else {
// attempted initiation within minimum rescue distance requires us to fly out to at least that distance
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minStartDistM) {
// climb above current height by buffer height, to at least 10m altitude
- rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, rescueState.sensor.currentAltitudeCm + (gpsRescueConfig()->initialClimbM * 100.0f));
+ rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, getAltitudeCm() + (gpsRescueConfig()->initialClimbM * 100.0f));
// note that the pitch controller will pitch forward to fly out to minStartDistM
// set the descent distance to half the minimum rescue distance. which we should have moved out to in the climb phase
rescueState.intent.descentDistanceM = 0.5f * gpsRescueConfig()->minStartDistM;
}
// otherwise behave as for a normal rescue
initialiseRescueValues ();
- returnAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
+ returnAltitudeLow = getAltitudeCm() < rescueState.intent.returnAltitudeCm;
rescueState.phase = RESCUE_ATTAIN_ALT;
}
}
@@ -829,13 +813,15 @@ void gpsRescueUpdate(void)
// gradually increment the target altitude until the craft reaches target altitude
// note that this can mean the target altitude may increase above returnAltitude if the craft lags target
// sanity check will abort if altitude gain is blocked for a cumulative period
- if (returnAltitudeLow == (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm)) {
+ if (returnAltitudeLow == (getAltitudeCm() < rescueState.intent.returnAltitudeCm)) {
// we started low, and still are low; also true if we started high, and still are too high
- const float altitudeStep = (returnAltitudeLow ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
- rescueState.intent.targetAltitudeCm += altitudeStep;
+ rescueState.intent.targetAltitudeStepCm = (returnAltitudeLow ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * taskIntervalSeconds;
+ rescueState.intent.targetAltitudeCm += rescueState.intent.targetAltitudeStepCm;
+
} else {
// target altitude achieved - move on to ROTATE phase, returning at target altitude
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
+ rescueState.intent.targetAltitudeStepCm = 0.0f;
// if initiated too close, do not rotate or do anything else until sufficiently far away that a 'normal' rescue can happen
if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->minStartDistM) {
rescueState.phase = RESCUE_ROTATE;
@@ -848,7 +834,7 @@ void gpsRescueUpdate(void)
case RESCUE_ROTATE:
if (rescueState.intent.yawAttenuator < 1.0f) { // acquire yaw authority over one second
- rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
+ rescueState.intent.yawAttenuator += taskIntervalSeconds;
}
if (rescueState.sensor.absErrorAngle < GPS_RESCUE_ALLOWED_YAW_RANGE) {
// enter fly home phase, and enable pitch, when the yaw angle error is small enough
@@ -862,12 +848,12 @@ void gpsRescueUpdate(void)
case RESCUE_FLY_HOME:
if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
- rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
+ rescueState.intent.yawAttenuator += taskIntervalSeconds;
}
// velocity PIDs are now active
// update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
const float targetVelocityError = gpsRescueConfig()->groundSpeedCmS - rescueState.intent.targetVelocityCmS;
- const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError;
+ const float velocityTargetStep = taskIntervalSeconds * targetVelocityError;
// velocityTargetStep is positive when starting low, negative when starting high
const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->groundSpeedCmS;
if (initialVelocityLow == targetVelocityIsLow) {
@@ -876,7 +862,7 @@ void gpsRescueUpdate(void)
}
// slowly introduce velocity iTerm accumulation at start, goes 0 ->1 with time constant 2.0s
- rescueState.intent.velocityItermRelax += 0.5f * rescueState.sensor.gpsRescueTaskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
+ rescueState.intent.velocityItermRelax += 0.5f * taskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
// there is always a lot of lag at the start, this gradual start avoids excess iTerm accumulation
rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax;
@@ -896,7 +882,7 @@ void gpsRescueUpdate(void)
case RESCUE_DESCENT:
// attenuate velocity and altitude targets while updating the heading to home
- if (isAltitudeLow()) {
+ if (isBelowLandingAltitude()) {
// enter landing mode once below landing altitude
rescueState.phase = RESCUE_LANDING;
rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
@@ -932,7 +918,7 @@ void gpsRescueUpdate(void)
}
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, lrintf(rescueState.intent.targetAltitudeCm));
- DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 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));
performSanityChecks();
@@ -953,12 +939,15 @@ float gpsRescueGetImuYawCogGain(void)
float gpsRescueGetThrottle(void)
{
- // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
- // We need to compensate for min_check since the throttle value set by gps rescue
- // is based on the raw rcCommand value commanded by the pilot.
+ // 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;
}
diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h
index 9fb2897dfc..205e325b0e 100644
--- a/src/main/flight/gps_rescue.h
+++ b/src/main/flight/gps_rescue.h
@@ -50,7 +50,6 @@ extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGR
void gpsRescueInit(void);
void gpsRescueUpdate(void);
void gpsRescueNewGpsData(void);
-
float gpsRescueGetYawRate(void);
float gpsRescueGetThrottle(void);
bool gpsRescueIsConfigured(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 79d9db4381..b484a500eb 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -50,7 +50,7 @@
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/rpm_filter.h"
#include "io/gps.h"
@@ -815,7 +815,7 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
// 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;
+ lowAltitudeSensitivity = (FLIGHT_MODE(ALT_HOLD_MODE) && isBelowLandingAltitude()) ? 1.5f : 1.0f;
#endif
// and disarm if accelerometer jerk exceeds threshold...
if ((fabsf(acc.accDelta) * lowAltitudeSensitivity) > pidRuntime.landingDisarmThreshold) {
diff --git a/src/main/flight/position.c b/src/main/flight/position.c
index d80b78b830..0482e9f649 100644
--- a/src/main/flight/position.c
+++ b/src/main/flight/position.c
@@ -34,6 +34,7 @@
#include "fc/runtime_config.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/imu.h"
#include "flight/pid.h"
@@ -48,14 +49,10 @@
#include "pg/pg_ids.h"
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 zeroedAltitudeCm = 0.0f;
static float zeroedAltitudeDerivative = 0.0f;
-#endif
static pt2Filter_t altitudeLpf;
static pt2Filter_t altitudeDerivativeLpf;
@@ -83,15 +80,13 @@ typedef enum {
GPS_ONLY
} altitudeSource_e;
-PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 5);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 6);
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)
@@ -202,9 +197,6 @@ 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
@@ -219,11 +211,24 @@ void calculateEstimatedAltitude(void)
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
#endif
DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
+ DEBUG_SET(DEBUG_ALTHOLD, 1, lrintf(zeroedAltitudeCm));
+ DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(zeroedAltitudeCm));
altitudeAvailable = haveGpsAlt || haveBaroAlt;
}
+
#endif //defined(USE_BARO) || defined(USE_GPS)
+float getAltitudeCm(void)
+{
+ return zeroedAltitudeCm;
+}
+
+float getAltitudeDerivative(void)
+{
+ return zeroedAltitudeDerivative; // cm/s
+}
+
bool isAltitudeAvailable(void) {
return altitudeAvailable;
}
@@ -233,16 +238,6 @@ int32_t getEstimatedAltitudeCm(void)
return lrintf(displayAltitudeCm);
}
-float getAltitude(void)
-{
- return zeroedAltitudeCm;
-}
-
-bool isAltitudeLow(void)
-{
- return altitudeIsLow;
-}
-
#ifdef USE_GPS
float getAltitudeAsl(void)
{
diff --git a/src/main/flight/position.h b/src/main/flight/position.h
index 0e349ea287..8800a6a5a6 100644
--- a/src/main/flight/position.h
+++ b/src/main/flight/position.h
@@ -29,17 +29,16 @@ 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);
+float getAltitudeCm(void);
+float getAltitudeDerivative(void);
void calculateEstimatedAltitude(void);
void positionInit(void);
int32_t getEstimatedAltitudeCm(void);
-float getAltitude(void);
-bool isAltitudeLow(void);
float getAltitudeAsl(void);
int16_t getEstimatedVario(void);
bool isAltitudeAvailable(void);
+
diff --git a/src/main/flight/position_control.c b/src/main/flight/position_control.c
new file mode 100644
index 0000000000..f87a6cccec
--- /dev/null
+++ b/src/main/flight/position_control.c
@@ -0,0 +1,50 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see .
+ */
+
+#include
+#include
+#include
+
+#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;
+}
diff --git a/src/main/flight/position_control.h b/src/main/flight/position_control.h
new file mode 100644
index 0000000000..0842aeb11c
--- /dev/null
+++ b/src/main/flight/position_control.h
@@ -0,0 +1,27 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see .
+ */
+
+#pragma once
+
+#include "pg/position_control.h"
+#include "flight/pid.h"
+
+void positionControlInit(const positionControlConfig_t *config);
+
+bool isBelowLandingAltitude(void);
+
+const pidCoefficient_t *getAltitudePidCoeffs(void);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 59b0fd3286..46a35c15ec 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -89,6 +89,7 @@
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/position.h"
+#include "flight/position_control.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
@@ -1545,7 +1546,7 @@ case MSP_NAME:
sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS);
sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
- sbufWriteU16(dst, positionConfig()->hover_throttle);
+ sbufWriteU16(dst, positionControlConfig()->hover_throttle);
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
sbufWriteU8(dst, gpsRescueConfig()->minSats);
@@ -1561,9 +1562,10 @@ case MSP_NAME:
break;
case MSP_GPS_RESCUE_PIDS:
- sbufWriteU16(dst, gpsRescueConfig()->throttleP);
- sbufWriteU16(dst, gpsRescueConfig()->throttleI);
- sbufWriteU16(dst, gpsRescueConfig()->throttleD);
+ sbufWriteU16(dst, positionControlConfig()->altitude_P);
+ sbufWriteU16(dst, positionControlConfig()->altitude_I);
+ sbufWriteU16(dst, positionControlConfig()->altitude_D);
+ // altitude_F not implemented yet
sbufWriteU16(dst, gpsRescueConfig()->velP);
sbufWriteU16(dst, gpsRescueConfig()->velI);
sbufWriteU16(dst, gpsRescueConfig()->velD);
@@ -2879,7 +2881,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src);
gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
- positionConfigMutable()->hover_throttle = sbufReadU16(src);
+ positionControlConfigMutable()->hover_throttle = sbufReadU16(src);
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
gpsRescueConfigMutable()->minSats = sbufReadU8(src);
if (sbufBytesRemaining(src) >= 6) {
@@ -2900,9 +2902,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
break;
case MSP_SET_GPS_RESCUE_PIDS:
- gpsRescueConfigMutable()->throttleP = sbufReadU16(src);
- gpsRescueConfigMutable()->throttleI = sbufReadU16(src);
- gpsRescueConfigMutable()->throttleD = sbufReadU16(src);
+ positionControlConfigMutable()->altitude_P = sbufReadU16(src);
+ positionControlConfigMutable()->altitude_I = sbufReadU16(src);
+ positionControlConfigMutable()->altitude_D = sbufReadU16(src);
+ // altitude_F not included in msp yet
gpsRescueConfigMutable()->velP = sbufReadU16(src);
gpsRescueConfigMutable()->velI = sbufReadU16(src);
gpsRescueConfigMutable()->velD = sbufReadU16(src);
diff --git a/src/main/pg/alt_hold.c b/src/main/pg/alt_hold.c
index ddf582d57f..28d291d4e5 100644
--- a/src/main/pg/alt_hold.c
+++ b/src/main/pg/alt_hold.c
@@ -29,12 +29,9 @@
#include "alt_hold.h"
-PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 4);
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,
diff --git a/src/main/pg/alt_hold.h b/src/main/pg/alt_hold.h
index 93d7db398d..bcffb8774c 100644
--- a/src/main/pg/alt_hold.h
+++ b/src/main/pg/alt_hold.h
@@ -25,9 +25,6 @@
#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;
diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c
index 24e498438e..f2c1ab908a 100644
--- a/src/main/pg/gps_rescue.c
+++ b/src/main/pg/gps_rescue.c
@@ -29,7 +29,7 @@
#include "gps_rescue.h"
-PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 7);
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
@@ -55,9 +55,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.sanityChecks = RESCUE_SANITY_FS_ONLY,
.minSats = 8,
- .throttleP = 15,
- .throttleI = 15,
- .throttleD = 20,
.velP = 8,
.velI = 40,
.velD = 12,
diff --git a/src/main/pg/gps_rescue.h b/src/main/pg/gps_rescue.h
index 14fe451e96..7597351d77 100644
--- a/src/main/pg/gps_rescue.h
+++ b/src/main/pg/gps_rescue.h
@@ -30,7 +30,6 @@ typedef struct gpsRescue_s {
uint16_t returnAltitudeM; // meters
uint16_t descentDistanceM; // meters
uint16_t groundSpeedCmS; // centimeters per second
- uint8_t throttleP, throttleI, throttleD;
uint8_t yawP;
uint16_t throttleMin;
uint16_t throttleMax;
diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h
index f01b339eb7..8614f9483f 100644
--- a/src/main/pg/pg_ids.h
+++ b/src/main/pg/pg_ids.h
@@ -83,6 +83,7 @@
#define PG_VTX_IO_CONFIG 57
#define PG_GPS_LAP_TIMER 58
#define PG_ALTHOLD_CONFIG 59
+#define PG_POSITION_CONTROL 60
// Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
diff --git a/src/main/pg/position_control.c b/src/main/pg/position_control.c
new file mode 100644
index 0000000000..e576220d57
--- /dev/null
+++ b/src/main/pg/position_control.c
@@ -0,0 +1,39 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+
+#include "flight/position_control.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "position_control.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
+
+PG_RESET_TEMPLATE(positionControlConfig_t, positionControlConfig,
+ .hover_throttle = 1275,
+ .landing_altitude_m = 4,
+ .altitude_P = 15,
+ .altitude_I = 15,
+ .altitude_D = 15,
+ .altitude_F = 15,
+);
diff --git a/src/main/pg/position_control.h b/src/main/pg/position_control.h
new file mode 100644
index 0000000000..7e5b70fa3e
--- /dev/null
+++ b/src/main/pg/position_control.h
@@ -0,0 +1,37 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include
+
+#include "pg/pg.h"
+
+typedef struct positionControlConfig_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 altitude_P;
+ uint8_t altitude_I;
+ uint8_t altitude_D;
+ uint8_t altitude_F;
+} positionControlConfig_t;
+
+PG_DECLARE(positionControlConfig_t, positionControlConfig);
+
diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc
index 1401363f7b..1efa95bd77 100644
--- a/src/test/unit/althold_unittest.cc
+++ b/src/test/unit/althold_unittest.cc
@@ -34,6 +34,8 @@ extern "C" {
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/position.h"
+ #include "flight/position_control.h"
+ #include "flight/pid.h"
#include "rx/rx.h"
@@ -41,6 +43,7 @@ extern "C" {
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
+ PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
extern altHoldState_t altHoldState;
@@ -97,6 +100,13 @@ TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter)
extern "C" {
acc_t acc;
+ 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 getAltitudeDerivative(void) { return 0.0f;}
+
void pt2FilterInit(pt2Filter_t *altHoldDeltaLpf, float) {
UNUSED(altHoldDeltaLpf);
}
@@ -109,8 +119,6 @@ extern "C" {
}
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];
diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc
index 44146f495f..52b1174c94 100644
--- a/src/test/unit/arming_prevention_unittest.cc
+++ b/src/test/unit/arming_prevention_unittest.cc
@@ -42,6 +42,7 @@ extern "C" {
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
+ #include "flight/position_control.h"
#include "flight/servos.h"
#include "io/beeper.h"
@@ -77,6 +78,7 @@ extern "C" {
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
+ PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t averageSystemLoadPercent = 0;
@@ -1134,17 +1136,17 @@ extern "C" {
bool isMotorProtocolEnabled(void) { return true; }
void pinioBoxTaskControl(void) {}
void schedulerSetNextStateTime(timeDelta_t) {}
- float getAltitude(void) { return 3000.0f; }
+
+ 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 getAltitudeDerivative(void) {return 0.0f;}
+
float pt1FilterGain(float, float) { return 0.5f; }
float pt2FilterGain(float, float) { return 0.1f; }
float pt3FilterGain(float, float) { return 0.1f; }
- void pt2FilterInit(pt2Filter_t *throttleDLpf, float) {
- UNUSED(throttleDLpf);
- }
- float pt2FilterApply(pt2Filter_t *throttleDLpf, float) {
- UNUSED(throttleDLpf);
- return 0.0f;
- }
void pt1FilterInit(pt1Filter_t *velocityDLpf, float) {
UNUSED(velocityDLpf);
}
@@ -1161,6 +1163,6 @@ extern "C" {
}
void getRcDeflectionAbs(void) {}
uint32_t getCpuPercentageLate(void) { return 0; }
- bool crashFlipSuccessful(void) {return false; }
- bool isAltitudeLow(void) {return false; }
+ bool crashFlipSuccessful(void) { return false; }
+ bool isBelowLandingAltitude(void) { return false; };
}
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index 4d441231a4..a06c0086be 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -46,8 +46,8 @@ extern "C" {
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/position.h"
+ #include "flight/position_control.h"
-
#include "io/gps.h"
#include "rx/rx.h"
@@ -74,6 +74,7 @@ extern "C" {
PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
+ PG_REGISTER(positionControlConfig_t, positionControlConfig, PG_POSITION_CONTROL, 0);
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = 0
@@ -438,7 +439,6 @@ extern "C" {
float getBaroAltitude(void) { return 3000.0f; }
float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
float getRcDeflectionAbs(int) { return 0.0f; }
-
void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) {
UNUSED(baroDerivativeLpf);
}
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index 52a675e392..9bcc165666 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -101,7 +101,7 @@ extern "C" {
float mixerGetRcThrottle() { return fabsf(simulatedMixerGetRcThrottle); }
- bool isAltitudeLow(void) { return false; }
+ bool isBelowLandingAltitude(void) { return false; }
void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; }