Shared altitude control parameters (#13884)

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 \

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_HOVER_THROTTLE, "%d",       positionControlConfig()->hover_throttle);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d",     positionConfig()->landing_altitude_m);
+        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);

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"

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)

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;
+    positionControlConfig_altitude_P = positionControlConfig()->altitude_P;
-    gpsRescueConfig_throttleI = gpsRescueConfig()->throttleI;
+    positionControlConfig_altitude_I = positionControlConfig()->altitude_I;
-    gpsRescueConfig_throttleD = gpsRescueConfig()->throttleD;
+    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;
+    positionControlConfigMutable()->altitude_P = positionControlConfig_altitude_P;
-    gpsRescueConfigMutable()->throttleI = gpsRescueConfig_throttleI;
+    positionControlConfigMutable()->altitude_I = positionControlConfig_altitude_I;
-    gpsRescueConfigMutable()->throttleD = gpsRescueConfig_throttleD;
+    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 } },
+    { "ALTITUDE P",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_P, 0, 255, 1 } },
-    { "THROTTLE I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleI, 0, 255, 1 } },
+    { "ALTITUDE I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &positionControlConfig_altitude_I, 0, 255, 1 } },
-    { "THROTTLE D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleD, 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 },

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

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"

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 {
+static float pidIntegral = 0.0f;
-    float kp;
+static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
-    float ki;
-    float kd;
-    float kf;
-    float previousAltitude;
-    float integral;
-} simplePid_t;
-
-simplePid_t simplePid;
 
 altHoldState_t altHoldState;
 
-#define ALT_HOLD_PID_P_SCALE  0.01f
-#define ALT_HOLD_PID_I_SCALE  0.003f
-#define ALT_HOLD_PID_D_SCALE  0.01f
-
-static pt2Filter_t altHoldDeltaLpf;
-static const float taskIntervalSeconds = 1.0f / ALTHOLD_TASK_RATE_HZ; // i.e. 0.01 s
-
 float altitudePidCalculate(void)
 {
     // * introductory notes *
-    // this is a simple PID controller with heuristic D boost and iTerm relax
+    // 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;
+    const float itermRelax = (fabsf(altitudeErrorCm) < itermNormalRange) ? 1.0f : 0.1f;
-    simplePid.integral += altErrorCm * taskIntervalSeconds * simplePid.ki * itermRelax;
+    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); 
+    pidIntegral = constrainf(pidIntegral, -200.0f, 200.0f); 
-    const float iOut = simplePid.integral;
+    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;
     
-    const float output = pOut + iOut + dOut + fOut;
+    // 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));
@@ -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;
+    pidIntegral = 0.0f;
-    simplePid.integral = 0.0f;
+    altHoldState.targetAltitudeCm = getAltitudeCm();
     altHoldState.targetAltitudeAdjustRate = 0.0f;
 }
 
 void altHoldInit(void)
 {
-    simplePidInit(
+    altHoldState.hoverThrottle = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
-        ALT_HOLD_PID_P_SCALE * altholdConfig()->alt_hold_pid_p,
-        ALT_HOLD_PID_I_SCALE * altholdConfig()->alt_hold_pid_i,
-        ALT_HOLD_PID_D_SCALE * altholdConfig()->alt_hold_pid_d,
-        0.01f * altholdConfig()->alt_hold_pid_d); // use D gain for feedforward with simple scaling
-        // the multipliers are base scale factors
-        // iTerm is relatively weak, intended to be slow moving to adjust baseline errors
-        // the Hover value is important otherwise takes time for iTerm to correct
-        // High P will wobble readily
-        // with these scalers, the same numbers as for GPS Rescue work OK for altHold
-        // the goal is to share these gain factors, if practical for all altitude controllers
-
-    //setup altitude D filter
-    const float cutoffHz = 0.01f * positionConfig()->altitude_d_lpf; // default 1Hz, time constant about 160ms
-    const float gain = pt2FilterGain(cutoffHz, taskIntervalSeconds);
-    pt2FilterInit(&altHoldDeltaLpf, gain);
-
-    altHoldState.hover = positionConfig()->hover_throttle - PWM_RANGE_MIN;
     altHoldState.isAltHoldActive = false;
     altHoldReset();
 }
@@ -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 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 * (positionConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 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
+    // check for enabling Alt Hold, otherwise do as little as possible while inactive
-    // calculate smoothed altitude Delta always, for effective value on 1st pass
-    altHoldState.measuredAltitudeCm = getAltitude();
-    float derivative = (simplePid.previousAltitude - altHoldState.measuredAltitudeCm) / taskIntervalSeconds; // cm/s
-    simplePid.previousAltitude = altHoldState.measuredAltitudeCm;
-
-    // smooth the derivative here to always have a current value, without delay due to filter lag
-    // this way we immediately have useful D on initialising the hold
-    altHoldState.smoothedVerticalVelocity = pt2FilterApply(&altHoldDeltaLpf, derivative);
-
-    DEBUG_SET(DEBUG_ALTHOLD, 1, lrintf(altHoldState.measuredAltitudeCm));
-
     altHoldProcessTransitions();
 
     if (altHoldState.isAltHoldActive) {
@@ -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

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 hoverThrottle;
-    float smoothedVerticalVelocity;
 } altHoldState_t;
 
 void altHoldInit(void);

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;
 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
+        // set the target altitude to the current altitude.
-        rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm;
+        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 altitudeErrorCm = (rescueState.intent.targetAltitudeCm - getAltitudeCm());
-    const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100.0f;
+    // height above target in cm (negative means too low)
-    // height above target in metres (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
+    // D component
-    float throttleD = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
+    const float throttleD = getAltitudeDerivative() * getAltitudePidCoeffs()->Kd * rescueState.intent.throttleDMultiplier;
-    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);
 
-    // 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
+    const float hoverOffset = positionControlConfig()->hover_throttle - PWM_RANGE_MIN;
-    tiltAdjustment *= (positionConfig()->hover_throttle - 1000);
-    // if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful
-    // too much and landings with lots of pitch adjustment, eg windy days, can be a problem
 
-    throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
+    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, 3, lrintf(tiltMultiplier * 100));
-    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(throttleD));
+    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 4, lrintf(throttleP));
-    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 4, lrintf(throttleI));
+    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 5, 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, 6, lrintf(throttleD));
-    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, 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 float altitudeCurrentCm = getAltitudeCm();
-    const timeDelta_t altitudeDataIntervalUs = cmpTimeUs(currentTimeUs, previousAltitudeDataTimeUs);
+    DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, lrintf(altitudeCurrentCm));
-    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));
     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.targetAltitudeStepCm = (returnAltitudeLow ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * taskIntervalSeconds;
-            rescueState.intent.targetAltitudeCm += altitudeStep;
+            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.
+    // Calculate the commanded throttle for use in the mixer. Output must be within range 0.0 - 1.0.
-    // We need to compensate for min_check since the throttle value set by gps rescue
+    // minCheck can be less than, or greater than, PWM_RANGE_MIN, but is usually at default of 1050
-    // is based on the raw rcCommand value commanded by the pilot.
+    // 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;
 }
 

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

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

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

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

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

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

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, positionControlConfig()->altitude_P);
-        sbufWriteU16(dst, gpsRescueConfig()->throttleI);
+        sbufWriteU16(dst, positionControlConfig()->altitude_I);
-        sbufWriteU16(dst, gpsRescueConfig()->throttleD);
+        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);
+        positionControlConfigMutable()->altitude_P = sbufReadU16(src);
-        gpsRescueConfigMutable()->throttleI = sbufReadU16(src);
+        positionControlConfigMutable()->altitude_I = sbufReadU16(src);
-        gpsRescueConfigMutable()->throttleD = 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);

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,

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;

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,

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;

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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,
+);

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

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

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 crashFlipSuccessful(void) { return false; }
-    bool isAltitudeLow(void) {return false; }
+    bool isBelowLandingAltitude(void) { return false; };
 }

src/test/unit/flight_imu_unittest.cc

@@ -46,7 +46,7 @@ extern "C" {
     #include "flight/pid.h"
     #include "flight/imu.h"
     #include "flight/position.h"
-
+    #include "flight/position_control.h"
 
     #include "io/gps.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);
     }

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