Position hold for 4.6 and Altitude Hold updates (#13975)

* autopilot naming, function sharing
* initial position hold setup
* get current gps location
* first position hold draft
* basic control scheme
* add debug values for testing
* first working example with smoothed acceleration
* add deadbands
* Re-organise included files and functions thanks Karate
* revise PID gains
* PIDJ distance controller, not velocity based. Remove airmode check
* sanity check notes
* fix pid bug and improve iTerm handling
* calculate PIDs independently for each axis, increas DJ gains
* adjust pids and smoothing; smoothing cutoff to settings
* force iTerm to zero when close to target
* allow earth referencing in alt and pos hold
* stop activation until throttle raised, but not require airmode
use wasThrottleRaised in place of isAirmodeActivated where airmode isn't really needed
* remove unnecessary debug
* block arming if poshold or althold switches are on
* basic sanity check and OSD warning
* allow user to fly without mag only if they reverse the default
* separate alt and pos hold deadbands
if poshold deadband is zero, sticks are ignored
if user tries to enable posHold and it can't work, they get stick with a deadband
* try to prevent position hold if no mag without valid IMU
fixes a bug in the last commit, also
* retaisn iTerm just attenuate the output
* struct for values
* reset position at start when slowed down, retain rotated iTerm back
* resolve bounceback and remove iTerm attenuation
* adjust PID gains to 30
* force unit tests sto work
* tidy up after merge
* Use mpif
* conditionally only do compass check if Mag is defined
* fixe defines and remove const
* comments to explain strange rc multiplier
* fix small omission when refactoring after throttle raised PR
* licence updates, refactoring from review comments
* fix issues when pos hold deadband is set to zero
* A for acceleration element, not J
* compact the posHoldUpdate() function - thanks K
* rename showPosHoldWarning to posHoldFailure
* Use a function in gps.c to initate the posHold activity on new GPS data
* Use autopilotAngle in place of posHoldAngle
* separate function for gpsHeading truth
* use FLIGHT_MODE(POS_HOLD_MODE) in place of isPosHoldRequested
* removed non-required definitions
* fix failure to initiate position hold from error in ifdef
* refactoring from reviews, rename posHoldReset
* move deadbands for pos and alt hold to their config files.
* comment
* fix for blackbox breakup of GPS values
reverts use a function in gps.c to initate the posHold activity on new GPS data
* fix for msp change for posHoldConfig
* try to constrain aggressiveness at start, smaller deadband
* allow greater overshoot at the start for high incoming speed
rename justStarted to isDeceleratingAtStart
* dynamically update smoothing at the start
* retain iTerm when moving sticks, to keep attitude in the wind
* fix unit tests
* finally retain iTerm correctly while moving sticks, but reset at start
* Fix iTerm reset and parameter rotation
* absolute rotation vs incremental rotation, fix spike after resetting target location
* don't rotate D or A, it reverses their sign inappropriately
* Block yaw, allow in CLI, option to apply yaw correction code
* restore debug
* calc D from groundspeed and drift angle
* add back some target based D
* Earth Frame iTerm vector
appears to work :-)
* fix unit test
* lower PID gains, slowly leak iTerm while sticks move
* earth ref Dterm, not from GPS Speed
smoother than using GPS Speed and heading
* stronger PIDs
* adjust debug
* shoehorn the unit tests
* Proper earth referencing, at last
* clean up a bit
* no need to duplicate wrapping done in sin_approx
* add note about PT1 gain on PT2 filters
* avoid unnecessary float conversions
* Remove unnecessary CLI testing params
* update PID gains, stronger tilt angle correction
* improved distance to target. thanks to demvlad
* Terminate start individually on each axis
added comments
* refactoring to avoid code duplication
* implement reviews, reduce PID gains
* upsampling filter at 5Hz
* warn if posHold mode requested but throttle not above airmode activation point
* disable angle feedforward in position hold
* rebase, pass unit test
* sequential PT1's, refactoring from reviews
* PID and filter revision
* bane of my life
* lenient sanity check, message for noMag, possible DA vector limit code
* replace angleTarget in pid.c only when autopilot is active
* rearrange status checks
* fix debug, tidy up EF axis names, add comments about sign and direction
* stop more cleanly, easier sanity check, phases, debugs complete
* extend sanity check distance while sticks move; refactor; comments
* fix instability on hard stop, allow activation after arming but before takeoff
* make altHoldState_t  local, getter to pass unit tests.
* hopefully the last cleanup of this test file
* implement review from PL - thank you!
* restore angle limiting in pid.c , max of 80 degrees allowed in CLI
* fixes after review changes weren't right
* fix braces
* limit max angle to 50 by vector length
* Fix curly brackets, comments and debug mistake
* in autopilot modes, allow up to 85 deg for pos hold
* limit pilot angle in position hold to half the configured position hold angle
* use smaller of angle_limit or half the autopilot limit
* increase alt_hold sensitivity 5x and narrow deadband to 20
* make altitude control 5x stronger with narrower deadband and new name
* add suggestions from recent reviews
* start autopilot gpsStamp at 0
* renaming variables
* reset the upsampling filters when resetting position control
* improved gpsStamp thanks PL
Also cleanup names and notes
* simplify altitude control
* rename to GPS_distances  to GPS_latLongVectors
* alt_hold respect zero deadband, hold when throttle is zero
* remove unused debug
* fix unit test
* re-name variables in alt_hold and update comments
* more flexible limit on target vs current altitude
* updates from reviews, thanks karate
* review changes from PL
* more updates from PL review
* rationalise GPS_latLongVectors
* remove static newGpsData and rescueThrottle
* Thanks, PL, for your review
* Modifications, but has a big twitch when sticks stop
* Re-naming, fix the twitch, minor changes
* remove unnecessary unit test reference
* sanity dist to 10m at full stop, send task interval for upsampling filter
* vector and parameter re-name
Co-Authored-By: Jan Post <19867640+KarateBrot@users.noreply.github.com>
* small changes from review
Co-Authored-By: Mark Haslinghuis <8344830+haslinghuis@users.noreply.github.com>
Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
* comment PL
Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
* fix ltm alt_hold flightMode
* NOINLINE some pid.c functions
* Revert "NOINLINE some pid.c functions"
This reverts commit 56a3f7cec2.
* fast_code_pref the wing functions in pid.c
* use fast_code_pref where won't break the build
* apply fast_code_pref correctly
* NOINLINE some pid.c functions
FAST_CODE_PREF for updatePIDAudio
add comment to FAST_CODE_PREF
FIx platform.h unit test issue
Wing functions all FAST_CODE_PREF
* revert FAST_CODE_PREF changes
* Reduce ITCM RAM footprint considerably
* multiple name changes and some refactoring
Thanks, PL
* small editorial change
* refactoring, thanks PL
Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
* 64 bytes to check crossing 180 deg longitude
Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
* try to fix build error
Co-Authored-By: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
* Revert "try to fix build error"
This reverts commit f926d26021.
* just guessing here
* Revert "just guessing here"
This reverts commit ebc240a325.
* use a null location at initialisation
* Revert "use a null location at initialisation"
This reverts commit b51ae1395d.
* revert more compact initialisation code due to SITL error otherwise
* fix wrapping when 180 lon meridian is crossed
* null location option from PL
* Revert "null location option from PL"
This reverts commit ad40e979bd.
* refactor PosHold start/stop code
* move Alt_Hold and Pos_Hold warnings ahead of Angle, update some comments
* use setTargetLocationByAxis, fix comments
* change from karatebrot review
Co-Authored-By: Jan Post <19867640+KarateBrot@users.noreply.github.com>
* things still to do
Co-Authored-By: Jan Post <19867640+KarateBrot@users.noreply.github.com>
* keep warning strings 12 or less characters
Co-Authored-By: Jan Post <19867640+KarateBrot@users.noreply.github.com>
* a few more
Co-Authored-By: Jan Post <19867640+KarateBrot@users.noreply.github.com>

---------

Co-authored-by: Jan Post <19867640+KarateBrot@users.noreply.github.com>
Co-authored-by: Mark Haslinghuis <8344830+haslinghuis@users.noreply.github.com>
Co-authored-by: Petr Ledvina <2318015+ledvinap@users.noreply.github.com>
Co-authored-by: Jay Blackman <blckmn@users.noreply.github.com>

mk/source.mk

@@ -25,6 +25,7 @@ PG_SRC = \
             pg/piniobox.c \
             pg/pinio.c \
             pg/pin_pull_up_down.c \
+            pg/pos_hold.c \
             pg/rcdevice.c \
             pg/rpm_filter.c \
             pg/rx.c \
@@ -164,6 +165,7 @@ COMMON_SRC = \
             flight/pid.c \
             flight/pid_init.c \
             flight/position.c \
+            flight/pos_hold.c \
             flight/rpm_filter.c \
             flight/servos.c \
             flight/servos_tricopter.c \

src/main/blackbox/blackbox.c

@@ -62,8 +62,6 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
-#include "flight/alt_hold.h"
-#include "flight/autopilot.h"
 #include "flight/failsafe.h"
 #include "flight/gps_rescue.h"
 #include "flight/mixer.h"
@@ -79,8 +77,12 @@
 
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
+
+#include "pg/alt_hold.h"
+#include "pg/autopilot.h"
 #include "pg/motor.h"
 #include "pg/rx.h"
+#include "pg/pos_hold.h"
 
 #include "rx/rx.h"
 
@@ -1685,25 +1687,32 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_HARDWARE, "%d",            accelerometerConfig()->acc_hardware);
 #endif
 #ifdef USE_BARO
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d",           barometerConfig()->baro_hardware);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d",        barometerConfig()->baro_hardware);
 #endif
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_SOURCE, "%d",      positionConfig()->altitude_source);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d",         positionConfig()->altitude_lpf);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d",       positionConfig()->altitude_d_lpf);
 
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d",         autopilotConfig()->landing_altitude_m);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d",           autopilotConfig()->hover_throttle);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d",             autopilotConfig()->throttle_min);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d",             autopilotConfig()->throttle_max);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d",               autopilotConfig()->altitude_P);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d",               autopilotConfig()->altitude_I);;
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d",               autopilotConfig()->altitude_D);;
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d",               autopilotConfig()->altitude_F);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d",     apConfig()->landing_altitude_m);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d",       apConfig()->hover_throttle);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d",         apConfig()->throttle_min);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d",         apConfig()->throttle_max);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d",           apConfig()->altitude_P);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d",           apConfig()->altitude_I);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d",           apConfig()->altitude_D);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d",           apConfig()->altitude_F);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d",           apConfig()->position_P);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d",           apConfig()->position_I);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d",           apConfig()->position_D);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d",           apConfig()->position_A);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d",      apConfig()->position_cutoff);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d",         apConfig()->max_angle);
 
 #ifdef USE_MAG
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d",           compassConfig()->mag_hardware);
 #endif
+
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d",  armingConfig()->gyro_cal_on_first_arm);
         BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d",       rxConfig()->airModeActivateThreshold);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d",     rxConfig()->serialrx_provider);
@@ -1807,7 +1816,13 @@ static bool blackboxWriteSysinfo(void)
 #endif // USE_GPS
 
 #ifdef USE_ALT_HOLD_MODE
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, "%d", altholdConfig()->alt_hold_target_adjust_rate);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND,    "%d", altHoldConfig()->alt_hold_deadband);
+#endif
+
+#ifdef USE_POS_HOLD_MODE
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND,    "%d", posHoldConfig()->pos_hold_deadband);
 #endif
 
 #ifdef USE_WING

src/main/build/debug.c

@@ -122,4 +122,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     [DEBUG_TASK] = "TASK",
     [DEBUG_GIMBAL] = "GIMBAL",
     [DEBUG_WING_SETPOINT] = "WING_SETPOINT",
+    [DEBUG_AUTOPILOT_POSITION] = "AUTOPILOT_POSITION",
 };

src/main/build/debug.h

@@ -124,6 +124,7 @@ typedef enum {
     DEBUG_TASK,
     DEBUG_GIMBAL,
     DEBUG_WING_SETPOINT,
+    DEBUG_AUTOPILOT_POSITION,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/cli.c

@@ -104,7 +104,6 @@ bool cliMode = false;
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/autopilot.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"

src/main/cli/settings.c

@@ -56,8 +56,6 @@
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 
-#include "flight/alt_hold.h"
-#include "flight/autopilot.h"
 #include "flight/failsafe.h"
 #include "flight/gps_rescue.h"
 #include "flight/imu.h"
@@ -80,6 +78,8 @@
 #include "osd/osd.h"
 
 #include "pg/adc.h"
+#include "pg/alt_hold.h"
+#include "pg/autopilot.h"
 #include "pg/beeper.h"
 #include "pg/beeper_dev.h"
 #include "pg/bus_i2c.h"
@@ -97,6 +97,7 @@
 #include "pg/pg_ids.h"
 #include "pg/pinio.h"
 #include "pg/piniobox.h"
+#include "pg/pos_hold.h"
 #include "pg/rx.h"
 #include "pg/rx_pwm.h"
 #include "pg/rx_spi.h"
@@ -1108,7 +1109,13 @@ 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_TARGET_ADJUST_RATE,    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_target_adjust_rate) },
+    { PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) },
+    { PARAM_NAME_ALT_HOLD_DEADBAND,          VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 },  PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_deadband) },
+#endif
+
+#ifdef USE_POS_HOLD_MODE
+    { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) },
+    { PARAM_NAME_POS_HOLD_DEADBAND,    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) },
 #endif
 
 // PG_PID_CONFIG
@@ -1198,7 +1205,7 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_ANGLE_P_GAIN,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
     { PARAM_NAME_ANGLE_FEEDFORWARD,     VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].F) },
     { PARAM_NAME_ANGLE_FF_SMOOTHING_MS, VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_feedforward_smoothing_ms) },
-    { PARAM_NAME_ANGLE_LIMIT,           VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 85 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_limit) },
+    { PARAM_NAME_ANGLE_LIMIT,           VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_limit) },
     { PARAM_NAME_ANGLE_EARTH_REF,       VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_earth_ref) },
 
     { PARAM_NAME_HORIZON_LEVEL_STRENGTH, VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
@@ -1842,14 +1849,20 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_ALTITUDE_D_LPF,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) },
 
 // PG_AUTOPILOT
-    { PARAM_NAME_LANDING_ALTITUDE,    VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, landing_altitude_m) },
-    { PARAM_NAME_HOVER_THROTTLE,      VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, hover_throttle) },
-    { PARAM_NAME_THROTTLE_MIN,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttle_min) },
-    { PARAM_NAME_THROTTLE_MAX,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttle_max) },
-    { PARAM_NAME_ALTITUDE_P,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_P) },
-    { PARAM_NAME_ALTITUDE_I,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_I) },
-    { PARAM_NAME_ALTITUDE_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_D) },
-    { PARAM_NAME_ALTITUDE_F,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_F) },
+    { PARAM_NAME_LANDING_ALTITUDE,    VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, landing_altitude_m) },
+    { PARAM_NAME_HOVER_THROTTLE,      VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(apConfig_t, hover_throttle) },
+    { PARAM_NAME_THROTTLE_MIN,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_min) },
+    { PARAM_NAME_THROTTLE_MAX,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_max) },
+    { PARAM_NAME_ALTITUDE_P,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, altitude_P) },
+    { PARAM_NAME_ALTITUDE_I,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, altitude_I) },
+    { PARAM_NAME_ALTITUDE_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, altitude_D) },
+    { PARAM_NAME_ALTITUDE_F,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, altitude_F) },
+    { PARAM_NAME_POSITION_P,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, position_P) },
+    { PARAM_NAME_POSITION_I,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, position_I) },
+    { PARAM_NAME_POSITION_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, position_D) },
+    { PARAM_NAME_POSITION_A,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(apConfig_t, position_A) },
+    { PARAM_NAME_POSITION_CUTOFF,     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 },    PG_AUTOPILOT, offsetof(apConfig_t, position_cutoff) },
+    { PARAM_NAME_AP_MAX_ANGLE,        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 },     PG_AUTOPILOT, offsetof(apConfig_t, max_angle) },
 
 // PG_MODE_ACTIVATION_CONFIG
 #if defined(USE_CUSTOM_BOX_NAMES)

src/main/cms/cms_menu_gps_rescue.c

@@ -37,10 +37,11 @@
 
 #include "config/config.h"
 
-#include "flight/autopilot.h"
-#include "flight/gps_rescue.h"
 #include "flight/position.h"
 
+#include "pg/autopilot.h"
+#include "pg/gps_rescue.h"
+
 static uint16_t gpsRescueConfig_minStartDistM; //meters
 static uint8_t gpsRescueConfig_altitudeMode;
 static uint16_t gpsRescueConfig_initialClimbM; // meters
@@ -52,16 +53,16 @@ static uint8_t gpsRescueConfig_angle; //degrees
 
 static uint16_t gpsRescueConfig_descentDistanceM; //meters
 static uint16_t gpsRescueConfig_descendRate;
-static uint8_t autopilotConfig_landingAltitudeM;
+static uint8_t apConfig_landingAltitudeM;
 
-static uint16_t autopilotConfig_throttleMin;
-static uint16_t autopilotConfig_throttleMax;
-static uint16_t autopilotConfig_hoverThrottle;
+static uint16_t apConfig_throttleMin;
+static uint16_t apConfig_throttleMax;
+static uint16_t apConfig_hoverThrottle;
 
 static uint8_t gpsRescueConfig_minSats;
 static uint8_t gpsRescueConfig_allowArmingWithoutFix;
 
-static uint8_t autopilotConfig_altitude_P, autopilotConfig_altitude_I, autopilotConfig_altitude_D, autopilotConfig_altitude_F;
+static uint8_t apConfig_altitude_P, apConfig_altitude_I, apConfig_altitude_D, apConfig_altitude_F;
 static uint8_t gpsRescueConfig_yawP;
 static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
 
@@ -72,10 +73,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
 {
     UNUSED(pDisp);
 
-    autopilotConfig_altitude_P = autopilotConfig()->altitude_P;
-    autopilotConfig_altitude_I = autopilotConfig()->altitude_I;
-    autopilotConfig_altitude_D = autopilotConfig()->altitude_D;
-    autopilotConfig_altitude_F = autopilotConfig()->altitude_F;
+    apConfig_altitude_P = apConfig()->altitude_P;
+    apConfig_altitude_I = apConfig()->altitude_I;
+    apConfig_altitude_D = apConfig()->altitude_D;
+    apConfig_altitude_F = apConfig()->altitude_F;
 
     gpsRescueConfig_yawP = gpsRescueConfig()->yawP;
 
@@ -94,10 +95,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
     UNUSED(pDisp);
     UNUSED(self);
 
-    autopilotConfigMutable()->altitude_P = autopilotConfig_altitude_P;
-    autopilotConfigMutable()->altitude_I = autopilotConfig_altitude_I;
-    autopilotConfigMutable()->altitude_D = autopilotConfig_altitude_D;
-    autopilotConfigMutable()->altitude_F = autopilotConfig_altitude_F;
+    apConfigMutable()->altitude_P = apConfig_altitude_P;
+    apConfigMutable()->altitude_I = apConfig_altitude_I;
+    apConfigMutable()->altitude_D = apConfig_altitude_D;
+    apConfigMutable()->altitude_F = apConfig_altitude_F;
 
     gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP;
 
@@ -115,10 +116,10 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
 {
     {"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
 
-    { "ALTITUDE P",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_P, 0, 200, 1 } },
-    { "ALTITUDE I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_I, 0, 200, 1 } },
-    { "ALTITUDE D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_D, 0, 200, 1 } },
-    { "ALTITUDE F",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_F, 0, 200, 1 } },
+    { "ALTITUDE P",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_P, 0, 200, 1 } },
+    { "ALTITUDE I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_I, 0, 200, 1 } },
+    { "ALTITUDE D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_D, 0, 200, 1 } },
+    { "ALTITUDE F",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_F, 0, 200, 1 } },
 
     { "YAW P",             OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 200, 1 } },
 
@@ -159,11 +160,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
 
     gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
     gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
-    autopilotConfig_landingAltitudeM = autopilotConfig()->landing_altitude_m;
+    apConfig_landingAltitudeM = apConfig()->landing_altitude_m;
 
-    autopilotConfig_throttleMin = autopilotConfig()->throttle_min;
-    autopilotConfig_throttleMax = autopilotConfig()->throttle_max;
-    autopilotConfig_hoverThrottle = autopilotConfig()->hover_throttle;
+    apConfig_throttleMin = apConfig()->throttle_min;
+    apConfig_throttleMax = apConfig()->throttle_max;
+    apConfig_hoverThrottle = apConfig()->hover_throttle;
 
     gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
     gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
@@ -187,11 +188,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
 
     gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
     gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
-    autopilotConfigMutable()->landing_altitude_m = autopilotConfig_landingAltitudeM;
+    apConfigMutable()->landing_altitude_m = apConfig_landingAltitudeM;
 
-    autopilotConfigMutable()->throttle_min = autopilotConfig_throttleMin;
-    autopilotConfigMutable()->throttle_max = autopilotConfig_throttleMax;
-    autopilotConfigMutable()->hover_throttle = autopilotConfig_hoverThrottle;
+    apConfigMutable()->throttle_min = apConfig_throttleMin;
+    apConfigMutable()->throttle_max = apConfig_throttleMax;
+    apConfigMutable()->hover_throttle = apConfig_hoverThrottle;
 
     gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
     gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
@@ -210,15 +211,15 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
 
     { "RETURN ALT      M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_returnAltitudeM, 2, 255, 1 } },
     { "RETURN SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_groundSpeedCmS, 0, 3000, 1 } },
-    { "PITCH ANGLE MAX",   OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_angle, 0, 60, 1 } },
+    { "PITCH ANGLE MAX",   OME_UINT8  | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &gpsRescueConfig_angle, 0, 60, 1 } },
 
     { "DESCENT DIST    M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } },
     { "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } },
-    { "LANDING ALT     M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_landingAltitudeM, 1, 15, 1 } },
+    { "LANDING ALT     M", OME_UINT8  | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &apConfig_landingAltitudeM, 1, 15, 1 } },
 
-    { "THROTTLE MIN",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMin, 1050, 1400, 1 } },
-    { "THROTTLE MAX",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMax, 1400, 2000, 1 } },
-    { "THROTTLE HOV",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_hoverThrottle, 1100, 1700, 1 } },
+    { "THROTTLE MIN",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMin, 1050, 1400, 1 } },
+    { "THROTTLE MAX",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMax, 1400, 2000, 1 } },
+    { "THROTTLE HOV",      OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_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/common/axis.h

@@ -42,6 +42,7 @@ typedef enum {
     AI_PITCH
 } angle_index_t;
 
-#define ANGLE_INDEX_COUNT 2
+#define RP_AXIS_COUNT 2
+#define EF_AXIS_COUNT 2
 
 #define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1)

src/main/common/maths.c

@@ -49,14 +49,27 @@
 #define sinPolyCoef7 -1.980661520e-4f                                          // Double: -1.980661520135080504411629636078917643846e-4
 #define sinPolyCoef9  2.600054768e-6f                                          // Double:  2.600054767890361277123254766503271638682e-6
 #endif
+
 float sin_approx(float x)
 {
-    int32_t xint = x;
-    if (xint < -32 || xint > 32) return 0.0f;                               // Stop here on error input (5 * 360 Deg)
-    while (x >  M_PIf) x -= (2.0f * M_PIf);                                 // always wrap input angle to -PI..PI
+    // Wrap angle to 2π with range [-π π]
+    x = fmodf(x, 2.0f * M_PIf);
+    // TO DO: these 'while' functions are not put into ITCM ram with current compiler flags
+    // A simpler 'if' function works, but gets put into ITCM ram, the extra 4% overflows F7xx ITCM
+    // The while function is retained only to avoid ITCM overflow for now
+    // ideally we should use the most efficient method, since sin_approx is used a LOT
+    // if (x <= -M_PIf) x += 2.0f * M_PIf;
+    // if (x > M_PIf) x -= 2.0f * M_PIf;
+    while (x >  M_PIf) x -= (2.0f * M_PIf);   // always wrap input angle to -PI..PI
     while (x < -M_PIf) x += (2.0f * M_PIf);
-    if (x >  (0.5f * M_PIf)) x =  (0.5f * M_PIf) - (x - (0.5f * M_PIf));   // We just pick -90..+90 Degree
-    else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
+
+    // Use axis symmetry around x = ±π/2 for polynomial outside of range [-π/2 π/2]
+    if (x > M_PIf / 2) {
+        x = M_PIf - x; // Reflect
+    } else if (x < -M_PIf / 2) {
+        x = -M_PIf - x; // Reflect
+    }
+
     float x2 = x * x;
     return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
 }

src/main/common/vector.c

@@ -96,6 +96,17 @@ vector2_t *vector2Normalize(vector2_t *result, const vector2_t *v)
     }
 }
 
+// rotate 2d vector by angle
+// angle is in radians and positive means counterclockwise
+vector2_t *vector2Rotate(vector2_t *result, const vector2_t *v, const float angle)
+{
+    vector2_t tmp;
+    tmp.x = v->x * cos_approx(angle) - v->y * sin_approx(angle);
+    tmp.y = v->x * sin_approx(angle) + v->y * cos_approx(angle);
+    *result = tmp;
+    return result;
+}
+ 
 bool vector3Equal(const vector3_t *a, const vector3_t *b)
 {
     return (a->x == b->x) && (a->y == b->y) && (a->z == b->z);

src/main/common/vector.h

@@ -57,6 +57,7 @@ float vector2Cross(const vector2_t *a, const vector2_t *b);
 float vector2NormSq(const vector2_t *v);
 float vector2Norm(const vector2_t *v);
 vector2_t *vector2Normalize(vector2_t *result, const vector2_t *v);
+vector2_t *vector2Rotate(vector2_t *result, const vector2_t *v, const float angle);
 
 bool vector3Equal(const vector3_t *a, const vector3_t *b);
 vector3_t *vector3Zero(vector3_t *v);

src/main/fc/core.c

@@ -62,6 +62,7 @@
 #include "flight/failsafe.h"
 #include "flight/gps_rescue.h"
 #include "flight/alt_hold.h"
+#include "flight/pos_hold.h"
 
 #if defined(USE_DYN_NOTCH_FILTER)
 #include "flight/dyn_notch_filter.h"
@@ -242,6 +243,7 @@ static bool accNeedsCalibration(void)
         if (isModeActivationConditionPresent(BOXANGLE) ||
             isModeActivationConditionPresent(BOXHORIZON) ||
             isModeActivationConditionPresent(BOXALTHOLD) ||
+            isModeActivationConditionPresent(BOXPOSHOLD) ||
             isModeActivationConditionPresent(BOXGPSRESCUE) ||
             isModeActivationConditionPresent(BOXCAMSTAB) ||
             isModeActivationConditionPresent(BOXCALIB) ||
@@ -318,6 +320,18 @@ void updateArmingStatus(void)
             unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
         }
 
+        if (IS_RC_MODE_ACTIVE(BOXALTHOLD)) {
+            setArmingDisabled(ARMING_DISABLED_ALTHOLD);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_ALTHOLD);
+        }
+
+        if (IS_RC_MODE_ACTIVE(BOXPOSHOLD)) {
+            setArmingDisabled(ARMING_DISABLED_POSHOLD);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_POSHOLD);
+        }
+
         if (calculateThrottleStatus() != THROTTLE_LOW) {
             setArmingDisabled(ARMING_DISABLED_THROTTLE);
         } else {
@@ -610,7 +624,7 @@ void tryArm(void)
         //beep to indicate arming
         if (featureIsEnabled(FEATURE_GPS)) {
             GPS_reset_home_position();
-
+            canUseGPSHeading = false; // block use of GPS Heading in position hold after each arm, until quad can set IMU to GPS COG
             if (STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
                 beeper(BEEPER_ARMING_GPS_FIX);
             } else {
@@ -768,7 +782,6 @@ uint8_t calculateThrottlePercentAbs(void)
 }
 
 static bool throttleRaised = false;
-
 bool wasThrottleRaised(void)
 {
     return throttleRaised;
@@ -1005,6 +1018,9 @@ void processRxModes(timeUs_t currentTimeUs)
         || failsafeIsActive()
 #ifdef USE_ALT_HOLD_MODE
         || FLIGHT_MODE(ALT_HOLD_MODE)
+#endif
+#ifdef USE_POS_HOLD_MODE
+        || FLIGHT_MODE(POS_HOLD_MODE)
 #endif
         ) && (sensors(SENSOR_ACC))) {
         // bumpless transfer to Level mode
@@ -1018,17 +1034,17 @@ void processRxModes(timeUs_t currentTimeUs)
     }
 
 #ifdef USE_ALT_HOLD_MODE
-    // only if armed
-    if (ARMING_FLAG(ARMED)
-        // and either the alt_hold switch is activated, or are in failsafe
-        && (IS_RC_MODE_ACTIVE(BOXALTHOLD) || failsafeIsActive())
-        // but not in GPS_RESCUE_MODE, ie if failsafe is active, must be in Landing Mode
+    // only if armed; can coexist with position hold
+    if (ARMING_FLAG(ARMED) 
+        // and not in GPS_RESCUE_MODE, to give it priority over Altitude Hold
         && !FLIGHT_MODE(GPS_RESCUE_MODE)
+        // and either the alt_hold switch is activated, or are in failsafe landing mode
+        && (IS_RC_MODE_ACTIVE(BOXALTHOLD) || failsafeIsActive())
         // and we have Acc for self-levelling
         && sensors(SENSOR_ACC)
         // and we have altitude data
         && isAltitudeAvailable()
-        // prevent activation until after takeoff
+        // but not until throttle is raised
         && wasThrottleRaised()) {
         if (!FLIGHT_MODE(ALT_HOLD_MODE)) {
             ENABLE_FLIGHT_MODE(ALT_HOLD_MODE);
@@ -1038,6 +1054,25 @@ void processRxModes(timeUs_t currentTimeUs)
     }
 #endif
 
+#ifdef USE_POS_HOLD_MODE
+    // only if armed; can coexist with altitude hold
+    if (ARMING_FLAG(ARMED) 
+        // and not in GPS_RESCUE_MODE, to give it priority over Position Hold
+        && !FLIGHT_MODE(GPS_RESCUE_MODE)
+        // and either the alt_hold switch is activated, or are in failsafe landing mode
+        && (IS_RC_MODE_ACTIVE(BOXPOSHOLD) || failsafeIsActive())
+        // and we have Acc for self-levelling
+        && sensors(SENSOR_ACC)
+        // but not until throttle is raised
+        && wasThrottleRaised()) {
+        if (!FLIGHT_MODE(POS_HOLD_MODE)) {
+            ENABLE_FLIGHT_MODE(POS_HOLD_MODE);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(POS_HOLD_MODE);
+    }
+#endif
+
     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode && sensors(SENSOR_ACC)) {
         DISABLE_FLIGHT_MODE(ANGLE_MODE);
         if (!FLIGHT_MODE(HORIZON_MODE)) {
@@ -1057,7 +1092,7 @@ void processRxModes(timeUs_t currentTimeUs)
     }
 #endif
 
-    if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE)) {
+    if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE | HORIZON_MODE)) {
         LED1_ON;
         // increase frequency of attitude task to reduce drift when in angle or horizon mode
         rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(acc.sampleRateHz / (float)imuConfig()->imu_process_denom));

src/main/fc/init.c

@@ -103,6 +103,7 @@
 #include "flight/pid.h"
 #include "flight/pid_init.h"
 #include "flight/position.h"
+#include "flight/pos_hold.h"
 #include "flight/servos.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
@@ -831,7 +832,7 @@ void init(void)
 #endif
 
     positionInit();
-    autopilotInit(autopilotConfig());
+    autopilotInit();
 
 #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
     vtxTableInit();
@@ -1012,6 +1013,10 @@ void init(void)
     altHoldInit();
 #endif
 
+#ifdef USE_POS_HOLD_MODE
+    posHoldInit();
+#endif
+
 #ifdef USE_GPS_RESCUE
     if (featureIsEnabled(FEATURE_GPS)) {
         gpsRescueInit();

src/main/fc/parameter_names.h

@@ -168,6 +168,12 @@
 #define PARAM_NAME_ALTITUDE_I "autopilot_altitude_I"
 #define PARAM_NAME_ALTITUDE_D "autopilot_altitude_D"
 #define PARAM_NAME_ALTITUDE_F "autopilot_altitude_F"
+#define PARAM_NAME_POSITION_P "autopilot_position_P"
+#define PARAM_NAME_POSITION_I "autopilot_position_I"
+#define PARAM_NAME_POSITION_D "autopilot_position_D"
+#define PARAM_NAME_POSITION_A "autopilot_position_A"
+#define PARAM_NAME_POSITION_CUTOFF "autopilot_position_cutoff"
+#define PARAM_NAME_AP_MAX_ANGLE "autopilot_max_angle"
 
 #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
 #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
@@ -242,8 +248,14 @@
 #endif // USE_GPS
 
 #ifdef USE_ALT_HOLD_MODE
-#define PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE "alt_hold_target_adjust_rate"
-#endif // USE_ALT_HOLD_MODE
+#define PARAM_NAME_ALT_HOLD_DEADBAND "alt_hold_deadband"
+#define PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE "alt_hold_throttle_response"
+#endif
+
+#ifdef USE_POS_HOLD_MODE
+#define PARAM_NAME_POS_HOLD_WITHOUT_MAG "pos_hold_without_mag"
+#define PARAM_NAME_POS_HOLD_DEADBAND "pos_hold_deadband"
+#endif
 
 #define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
 #define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"

src/main/fc/rc.c

@@ -27,7 +27,6 @@
 
 #include "build/debug.h"
 
-#include "common/axis.h"
 #include "common/utils.h"
 #include "common/vector.h"
 
@@ -48,7 +47,6 @@
 #include "flight/pid_init.h"
 
 #include "pg/rx.h"
-
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
@@ -717,7 +715,6 @@ FAST_CODE void processRcCommand(void)
                 } else {
                     rcCommandf = rcCommand[axis] / rcCommandDivider;
                 }
-
                 rcDeflection[axis] = rcCommandf;
                 const float rcCommandfAbs = fabsf(rcCommandf);
                 rcDeflectionAbs[axis] = rcCommandfAbs;
@@ -752,26 +749,15 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
     isRxDataNew = true;
 
     for (int axis = 0; axis < 3; axis++) {
-
-        float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f);
+        float rc = constrainf(rcData[axis] - rxConfig()->midrc, -500.0f, 500.0f); // -500 to 500
+        float rcDeadband = 0;
         if (axis == ROLL || axis == PITCH) {
-            if (tmp > rcControlsConfig()->deadband) {
-                tmp -= rcControlsConfig()->deadband;
-            } else {
-                tmp = 0;
-            }
-            rcCommand[axis] = tmp;
+            rcDeadband = rcControlsConfig()->deadband;
         } else {
-            if (tmp > rcControlsConfig()->yaw_deadband) {
-                tmp -= rcControlsConfig()->yaw_deadband;
-            } else {
-                tmp = 0;
-            }
-            rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
-        }
-        if (rcData[axis] < rxConfig()->midrc) {
-            rcCommand[axis] = -rcCommand[axis];
+            rcDeadband  = rcControlsConfig()->yaw_deadband;
+            rc *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
         }
+        rcCommand[axis] = fapplyDeadband(rc, rcDeadband);
     }
 
     int32_t tmp;
@@ -812,7 +798,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
 
         rcCommandBuff.x = rcCommand[ROLL];
         rcCommandBuff.y = rcCommand[PITCH];
-        if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
+        if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
             rcCommandBuff.z = rcCommand[YAW];
         } else {
             rcCommandBuff.z = 0;
@@ -820,7 +806,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
         imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
         rcCommand[ROLL] = rcCommandBuff.x;
         rcCommand[PITCH] = rcCommandBuff.y;
-        if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
+        if (!FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE | HORIZON_MODE | GPS_RESCUE_MODE)) {
             rcCommand[YAW] = rcCommandBuff.z;
         }
     }

src/main/fc/rc_controls.c

@@ -73,13 +73,11 @@ static bool isUsingSticksToArm = true;
 
 float rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
-PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1);
 
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
     .deadband = 0,
     .yaw_deadband = 0,
-    .alt_hold_deadband = 40,
-    .alt_hold_fast_change = 1,
     .yaw_control_reversed = false,
 );
 

src/main/fc/rc_controls.h

@@ -22,6 +22,7 @@
 
 #include <stdbool.h>
 
+#include "common/axis.h"
 #include "common/filter.h"
 #include "pg/pg.h"
 
@@ -87,7 +88,7 @@ extern float rcCommand[4];
 typedef struct rcSmoothingFilter_s {
     bool filterInitialized;
     pt3Filter_t filterSetpoint[4];
-    pt3Filter_t filterRcDeflection[2];
+    pt3Filter_t filterRcDeflection[RP_AXIS_COUNT];
     pt3Filter_t filterFeedforward[3];
 
     uint8_t setpointCutoffSetting;
@@ -110,9 +111,7 @@ typedef struct rcSmoothingFilter_s {
 typedef struct rcControlsConfig_s {
     uint8_t deadband;                       // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
     uint8_t yaw_deadband;                   // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
-    uint8_t alt_hold_deadband;              // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
-    uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
-    bool yaw_control_reversed;            // invert control direction of yaw
+    bool yaw_control_reversed;              // invert control direction of yaw
 } rcControlsConfig_t;
 
 PG_DECLARE(rcControlsConfig_t, rcControlsConfig);

src/main/fc/rc_modes.c

@@ -56,7 +56,7 @@ static uint8_t activeMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
 static int activeLinkedMacCount = 0;
 static uint8_t activeLinkedMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
 
-PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 3);
+PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 4);
 
 #if defined(USE_CUSTOM_BOX_NAMES)
 PG_REGISTER(modeActivationConfig_t, modeActivationConfig, PG_MODE_ACTIVATION_CONFIG, 0);

src/main/fc/rc_modes.h

@@ -37,6 +37,7 @@ typedef enum {
     BOXHEADFREE,
     BOXPASSTHRU,
     BOXFAILSAFE,
+    BOXPOSHOLD,
     BOXGPSRESCUE,
     BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE,
 

src/main/fc/runtime_config.c

@@ -59,7 +59,10 @@ const char *armingDisableFlagNames[]= {
     "DSHOT_BBANG",
     "NO_ACC_CAL",
     "MOTOR_PROTO",
-    "ARMSWITCH",
+    "FLIP_SWITCH",
+    "ALT_HOLD_SW",
+    "POS_HOLD_SW",
+    "ARM_SWITCH",
 };
 
 static armingDisableFlags_e armingDisableFlags = 0;

src/main/fc/runtime_config.h

@@ -66,7 +66,9 @@ typedef enum {
     ARMING_DISABLED_ACC_CALIBRATION = (1 << 23),
     ARMING_DISABLED_MOTOR_PROTOCOL  = (1 << 24),
     ARMING_DISABLED_CRASHFLIP       = (1 << 25),
-    ARMING_DISABLED_ARM_SWITCH      = (1 << 26), // Needs to be the last element, since it's always activated if one of the others is active when arming
+    ARMING_DISABLED_ALTHOLD         = (1 << 26),
+    ARMING_DISABLED_POSHOLD         = (1 << 27),
+    ARMING_DISABLED_ARM_SWITCH      = (1 << 28) // Needs to be the last element, since it's always activated if one of the others is active when arming
 } armingDisableFlags_e;
 
 #define ARMING_DISABLE_FLAGS_COUNT (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1)
@@ -84,7 +86,7 @@ typedef enum {
     MAG_MODE        = (1 << 2),
     ALT_HOLD_MODE    = (1 << 3),
 //    GPS_HOME_MODE   = (1 << 4),
-//    GPS_HOLD_MODE   = (1 << 5),
+    POS_HOLD_MODE   = (1 << 5),
     HEADFREE_MODE   = (1 << 6),
 //    UNUSED_MODE     = (1 << 7), // old autotune
     PASSTHRU_MODE   = (1 << 8),
@@ -105,7 +107,8 @@ extern uint16_t flightModeFlags;
    [BOXANGLE]       = LOG2(ANGLE_MODE),                  \
    [BOXHORIZON]     = LOG2(HORIZON_MODE),                \
    [BOXMAG]         = LOG2(MAG_MODE),                    \
-   [BOXALTHOLD]     = LOG2(ALT_HOLD_MODE),                \
+   [BOXALTHOLD]     = LOG2(ALT_HOLD_MODE),               \
+   [BOXPOSHOLD]     = LOG2(POS_HOLD_MODE),               \
    [BOXHEADFREE]    = LOG2(HEADFREE_MODE),               \
    [BOXPASSTHRU]    = LOG2(PASSTHRU_MODE),               \
    [BOXFAILSAFE]    = LOG2(FAILSAFE_MODE),               \

src/main/fc/tasks.c

@@ -53,12 +53,13 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/alt_hold.h"
 #include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/position.h"
-#include "flight/alt_hold.h"
+#include "flight/pos_hold.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -202,7 +203,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
         break;
 
     case RX_STATE_UPDATE:
-        // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
+        // updateRcCommands sets rcCommand, which is needed by updateAltHold and updateSonarAltHoldState
         updateRcCommands();
         updateArmingStatus();
 
@@ -378,7 +379,11 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #endif
 
 #ifdef USE_ALT_HOLD_MODE
-    [TASK_ALTHOLD] = DEFINE_TASK("ALTHOLD", NULL, NULL, updateAltHoldState, TASK_PERIOD_HZ(ALTHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW),
+    [TASK_ALTHOLD] = DEFINE_TASK("ALTHOLD", NULL, NULL, updateAltHold, TASK_PERIOD_HZ(ALTHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW),
+#endif
+
+#ifdef USE_POS_HOLD_MODE
+    [TASK_POSHOLD] = DEFINE_TASK("POSHOLD", NULL, NULL, updatePosHold, TASK_PERIOD_HZ(POSHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW),
 #endif
 
 #ifdef USE_MAG
@@ -540,7 +545,11 @@ void tasksInit(void)
 #endif
 
 #ifdef USE_ALT_HOLD_MODE
-    setTaskEnabled(TASK_ALTHOLD, true);
+    setTaskEnabled(TASK_ALTHOLD, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS));
+#endif
+
+#ifdef USE_POS_HOLD_MODE
+    setTaskEnabled(TASK_POSHOLD, featureIsEnabled(FEATURE_GPS));
 #endif
 
 #ifdef USE_MAG

src/main/flight/alt_hold.c

@@ -23,63 +23,62 @@
 #include "build/debug.h"
 #include "common/maths.h"
 #include "config/config.h"
+
 #include "fc/rc.h"
 #include "fc/runtime_config.h"
+
 #include "flight/autopilot.h"
 #include "flight/failsafe.h"
 #include "flight/position.h"
+
 #include "rx/rx.h"
+#include "pg/autopilot.h"
 
 #include "alt_hold.h"
 
 static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
 
-altHoldState_t altHoldState;
+typedef struct {
+    bool isActive;
+    float targetAltitudeCm;
+    float maxVelocity;
+    float targetVelocity;
+    float deadband;
+    bool allowStickAdjustment;
+} altHoldState_t;
 
-void controlAltitude(void)
-{
-    // boost D by 'increasing apparent velocity' when vertical velocity exceeds 5 m/s ( D of 75 on defaults)
-    // usually we don't see fast ascend/descend rates if the altitude hold starts under stable conditions
-    // this is important primarily to arrest pre-existing fast drops or climbs at the start;
-    float verticalVelocity = getAltitudeDerivative(); // cm/s altitude derivative is always available
-    const float kinkPoint = 500.0f; // velocity at which D should start to increase
-    const float kinkPointAdjustment = kinkPoint * 2.0f; // Precompute constant
-    const float sign = (verticalVelocity > 0) ? 1.0f : -1.0f;
-    if (fabsf(verticalVelocity) > kinkPoint) {
-        verticalVelocity = verticalVelocity * 3.0f - sign * kinkPointAdjustment;
-    }
-
-    //run the function in autopilot.c that calculates the PIDs and drives the motors
-    altitudeControl(altHoldState.targetAltitudeCm, taskIntervalSeconds, verticalVelocity, altHoldState.targetAltitudeAdjustRate);
-}
+altHoldState_t altHold;
 
 void altHoldReset(void)
 {
     resetAltitudeControl();
-    altHoldState.targetAltitudeCm = getAltitudeCm();
-    altHoldState.targetAltitudeAdjustRate = 0.0f;
+    altHold.targetAltitudeCm = getAltitudeCm();
+    altHold.targetVelocity = 0.0f;
 }
 
 void altHoldInit(void)
 {
-    altHoldState.isAltHoldActive = false;
+    altHold.isActive = false;
+    altHold.deadband = altHoldConfig()->alt_hold_deadband / 100.0f;
+    altHold.allowStickAdjustment = altHoldConfig()->alt_hold_deadband;
+    altHold.maxVelocity = altHoldConfig()->alt_hold_adjust_rate * 10.0f; // 50 in CLI means 500cm/s
     altHoldReset();
 }
 
 void altHoldProcessTransitions(void) {
 
     if (FLIGHT_MODE(ALT_HOLD_MODE)) {
-        if (!altHoldState.isAltHoldActive) {
+        if (!altHold.isActive) {
             altHoldReset();
-            altHoldState.isAltHoldActive = true;
+            altHold.isActive = true;
         }
     } else {
-        altHoldState.isAltHoldActive = false;
+        altHold.isActive = false;
     }
 
     // ** the transition out of alt hold (exiting altHold) may be rough.  Some notes... **
     // The original PR had a gradual transition from hold throttle to pilot control throttle
-    // using !(altHoldRequested && altHoldState.isAltHoldActive) to run an exit function
+    // using !(altHoldRequested && altHold.isAltHoldActive) to run an exit function
     // a cross-fade factor was sent to mixer.c based on time since the flight mode request was terminated
     // it was removed primarily to simplify this PR
 
@@ -90,65 +89,68 @@ void altHoldProcessTransitions(void) {
 
 void altHoldUpdateTargetAltitude(void)
 {
-    // The user can raise or lower the target altitude with throttle up;  there is a big deadband.
-    // Max rate for climb/descend is 1m/s by default (up to 2.5 is allowed but overshoots a fair bit)
-    // If set to zero, the throttle has no effect.
+    // User can adjust the target altitude with throttle, but only when
+    // - throttle is outside deadband, and 
+    // - throttle is not low (zero), and 
+    // - deadband is not configured to zero
 
-    // Some people may not like throttle being able to change the target altitude, because:
-    // - with throttle adjustment, hitting the switch won't always hold altitude if throttle is bumped
-    // - eg if the throttle is bumped low accidentally, quad will start descending.
-    // On the plus side:
-    // - the pilot has control nice control over altitude, and the deadband is wide
-    // - Slow controlled descents are possible, eg for landing
-    // - fine-tuning height is possible, eg if there is slow sensor drift
-    // - to keep the craft stable, throttle must be in the deadband, making exits smoother
+    float stickFactor = 0.0f;
 
-    const float rcThrottle = rcCommand[THROTTLE];
+    if (altHold.allowStickAdjustment && calculateThrottleStatus() != THROTTLE_LOW) {
+        const float rcThrottle = rcCommand[THROTTLE];
+        const float lowThreshold = apConfig()->hover_throttle - altHold.deadband * (apConfig()->hover_throttle - PWM_RANGE_MIN);
+        const float highThreshold = apConfig()->hover_throttle + altHold.deadband * (PWM_RANGE_MAX - apConfig()->hover_throttle);
 
-    const float lowThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300
-    const float highThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300
-
-    float throttleAdjustmentFactor = 0.0f;
-    if (rcThrottle < lowThreshold) {
-        throttleAdjustmentFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f);
-    } else if (rcThrottle > highThreshold) {
-        throttleAdjustmentFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f);
+        if (rcThrottle < lowThreshold) {
+            stickFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f);
+        } else if (rcThrottle > highThreshold) {
+            stickFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f);
+        }
     }
-    // if failsafe is active, and we get here, we are in failsafe landing mode
+
+    // if failsafe is active, and we get here, we are in failsafe landing mode, it controls throttle
     if (failsafeIsActive()) {
         // descend at up to 10 times faster when high
-        // default landing time is now 60s; need to get the quad down in this time from reasonable height
-        // need a rapid descent if high to avoid that timeout, and must slow down closer to ground
+        // default landing timeout is now 60s; must to get the quad down within this limit
+        // need a rapid descent when initiated high, and must slow down closer to ground
         // this code doubles descent rate at 20m, to max 10x (10m/s on defaults) at 200m
-        // user should be able to descend within 60s from around 150m high without disarming, on defaults
         // the deceleration may be a bit rocky if it starts very high up
         // constant (set) deceleration target in the last 2m
-        throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
+        stickFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
     }
-    altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate;
+    altHold.targetVelocity = stickFactor * altHold.maxVelocity;
 
-    // if taskRate is 100Hz, default adjustRate of 100 adds/subtracts 1m every second, or 1cm per task run, at full stick position
-    altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate  * taskIntervalSeconds;
+    // prevent stick input from moving target altitude too far away from current altitude
+    // otherwise it can be difficult to bring target altitude close to current altitude in a reasonable time
+    // using maxVelocity means the stick can bring altitude target to current within 1s
+    // this constrains the P and I response to user target changes, but not D of F responses
+    // Range is compared to distance that might be traveled in one second
+    if (fabsf(getAltitudeCm() - altHold.targetAltitudeCm) < altHold.maxVelocity * 1.0 /* s */) {
+        altHold.targetAltitudeCm += altHold.targetVelocity * taskIntervalSeconds;
+    }
 }
 
 void altHoldUpdate(void)
 {
     // check if the user has changed the target altitude using sticks
-    if (altholdConfig()->alt_hold_target_adjust_rate) {
+    if (altHoldConfig()->alt_hold_adjust_rate) {
         altHoldUpdateTargetAltitude();
     }
-
-    controlAltitude();
+    altitudeControl(altHold.targetAltitudeCm, taskIntervalSeconds, altHold.targetVelocity);
 }
 
-void updateAltHoldState(timeUs_t currentTimeUs) {
+void updateAltHold(timeUs_t currentTimeUs) {
     UNUSED(currentTimeUs);
 
     // check for enabling Alt Hold, otherwise do as little as possible while inactive
     altHoldProcessTransitions();
 
-    if (altHoldState.isAltHoldActive) {
+    if (altHold.isActive) {
         altHoldUpdate();
     }
 }
+
+bool isAltHoldActive(void) {
+    return altHold.isActive;
+}
 #endif

src/main/flight/alt_hold.h

@@ -24,13 +24,8 @@
 
 #define ALTHOLD_TASK_RATE_HZ 100         // hz
 
-typedef struct {
-    bool isAltHoldActive;
-    float targetAltitudeCm;
-    float targetAltitudeAdjustRate;
-} altHoldState_t;
-
 void altHoldInit(void);
-void updateAltHoldState(timeUs_t currentTimeUs);
+void updateAltHold(timeUs_t currentTimeUs);
+bool isAltHoldActive(void);
 
 #endif

src/main/flight/autopilot.c

@@ -22,38 +22,149 @@
 
 #include "platform.h"
 #include "build/debug.h"
+#include "common/axis.h"
+#include "common/filter.h"
 #include "common/maths.h"
+#include "common/vector.h"
+#include "fc/rc.h"
+#include "fc/runtime_config.h"
 
 #include "flight/imu.h"
-#include "flight/pid.h"
 #include "flight/position.h"
 #include "rx/rx.h"
+#include "sensors/gyro.h"
 
+#include "pg/autopilot.h"
 #include "autopilot.h"
 
 #define ALTITUDE_P_SCALE  0.01f
 #define ALTITUDE_I_SCALE  0.003f
 #define ALTITUDE_D_SCALE  0.01f
 #define ALTITUDE_F_SCALE  0.01f
+#define POSITION_P_SCALE  0.0012f
+#define POSITION_I_SCALE  0.0001f
+#define POSITION_D_SCALE  0.0015f
+#define POSITION_A_SCALE  0.0008f
+#define UPSAMPLING_CUTOFF_HZ 5.0f
 
 static pidCoefficient_t altitudePidCoeffs;
+static pidCoefficient_t positionPidCoeffs;
+
 static float altitudeI = 0.0f;
 static float throttleOut = 0.0f;
 
-void autopilotInit(const autopilotConfig_t *config)
+typedef struct efPidAxis_s {
+    bool isStopping;
+    float previousDistance;
+    float previousVelocity;
+    float integral;
+    pt1Filter_t velocityLpf;
+    pt1Filter_t accelerationLpf;
+} efPidAxis_t;
+
+typedef enum {
+    // axes are for ENU system; it is different from current Betaflight code
+    LON = 0,   // X, east
+    LAT        // Y, north
+} axisEF_e;
+
+typedef struct autopilotState_s {
+    gpsLocation_t targetLocation;       // active / current target
+    float sanityCheckDistance;
+    float upsampleLpfGain;              // for the Body Frame upsample filter for pitch and roll
+    float vaLpfCutoff;                  // velocity + acceleration lowpass filter cutoff
+    bool sticksActive;
+    float maxAngle;
+    vector2_t pidSumBF;                 // pid output, updated on each GPS update, rotated to body frame
+    pt3Filter_t upsampleLpfBF[RP_AXIS_COUNT];    // upsampling filter
+    efPidAxis_t efAxis[EF_AXIS_COUNT];
+} autopilotState_t;
+
+static autopilotState_t ap = {
+    .sanityCheckDistance = 1000.0f,
+    .upsampleLpfGain = 1.0f,
+    .vaLpfCutoff = 1.0f,
+    .sticksActive = false,
+};
+
+float autopilotAngle[RP_AXIS_COUNT];
+
+static void resetEFAxisFilters(efPidAxis_t* efAxis, const float vaGain)
 {
-    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;
+    pt1FilterInit(&efAxis->velocityLpf, vaGain);
+    pt1FilterInit(&efAxis->accelerationLpf, vaGain);
+}
+
+void resetEFAxisParams(efPidAxis_t *efAxis, const float vaGain)
+{
+    // at start only
+    resetEFAxisFilters(efAxis, vaGain);
+    efAxis->isStopping = true; // Enter starting (deceleration) 'phase'
+    efAxis->integral = 0.0f;
+}
+
+static void resetUpsampleFilters(void)
+{
+    for (unsigned i = 0; i < ARRAYLEN(ap.upsampleLpfBF); i++) {
+        pt3FilterInit(&ap.upsampleLpfBF[i], ap.upsampleLpfGain);
+    }
+}
+
+// get sanity distance based on speed
+static inline float sanityCheckDistance(const float gpsGroundSpeedCmS)
+{
+    return fmaxf(1000.0f, gpsGroundSpeedCmS * 2.0f);
+    // distance flown in 2s at current speed. with minimum of 10m
+}
+
+void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz)
+{
+    // from pos_hold.c (or other client) when initiating position hold at target location
+    ap.targetLocation = *initialTargetLocation;
+    ap.sticksActive = false;
+    // set sanity check distance according to groundspeed at start, minimum of 10m
+    ap.sanityCheckDistance = sanityCheckDistance(gpsSol.groundSpeed);
+    for (unsigned i = 0; i < ARRAYLEN(ap.efAxis); i++) {
+        // clear anything stored in the filter at first call
+        resetEFAxisParams(&ap.efAxis[i], 1.0f);
+    }
+    const float taskInterval = 1.0f / taskRateHz;
+    ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, taskInterval); // 5Hz; normally at 100Hz task rate
+    resetUpsampleFilters(); // clear accumlator from previous iterations
+}
+
+void autopilotInit(void)
+{
+    const apConfig_t *cfg = apConfig();
+
+    ap.sticksActive = false;
+    ap.maxAngle = cfg->max_angle;
+    altitudePidCoeffs.Kp = cfg->altitude_P * ALTITUDE_P_SCALE;
+    altitudePidCoeffs.Ki = cfg->altitude_I * ALTITUDE_I_SCALE;
+    altitudePidCoeffs.Kd = cfg->altitude_D * ALTITUDE_D_SCALE;
+    altitudePidCoeffs.Kf = cfg->altitude_F * ALTITUDE_F_SCALE;
+    positionPidCoeffs.Kp = cfg->position_P * POSITION_P_SCALE;
+    positionPidCoeffs.Ki = cfg->position_I * POSITION_I_SCALE;
+    positionPidCoeffs.Kd = cfg->position_D * POSITION_D_SCALE;
+    positionPidCoeffs.Kf = cfg->position_A * POSITION_A_SCALE; // Kf used for acceleration
+    // initialise filters with approximate filter gains; location isn't used at this point.
+    ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate at init
+    resetUpsampleFilters();
+    // Initialise PT1 filters for velocity and acceleration in earth frame axes
+    ap.vaLpfCutoff = cfg->position_cutoff * 0.01f;
+    const float vaGain = pt1FilterGain(ap.vaLpfCutoff,  0.1f); // assume 10Hz GPS connection at start; value is overwritten before first filter use
+    for (unsigned i = 0; i < ARRAYLEN(ap.efAxis); i++) {
+        resetEFAxisFilters(&ap.efAxis[i], vaGain);
+    }
 }
 
 void resetAltitudeControl (void) {
     altitudeI = 0.0f;
 }
 
-void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep) {
-
+void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep)
+{
+    const float verticalVelocityCmS = getAltitudeDerivative();
     const float altitudeErrorCm = targetAltitudeCm - getAltitudeCm();
     const float altitudeP = altitudeErrorCm * altitudePidCoeffs.Kp;
 
@@ -63,20 +174,33 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float vertical
     // limit iTerm to not more than 200 throttle units
     altitudeI = constrainf(altitudeI, -200.0f, 200.0f);
 
-    const float altitudeD = verticalVelocity * altitudePidCoeffs.Kd;
+    // increase D when velocity is high, typically when initiating hold at high vertical speeds
+    // 1.0 when less than 5 m/s, 2x at 10m/s, 2.5 at 20 m/s, 2.8 at 50 m/s, asymptotes towards max 3.0.
+    float dBoost = 1.0f;
+    {
+        const float startValue = 500.0f; // velocity at which D should start to increase
+        const float altDeriv = fabsf(verticalVelocityCmS);
+        if (altDeriv > startValue) {
+            const float ratio = altDeriv / startValue;
+            dBoost = (3.0f * ratio - 2.0f) / ratio;
+        }
+    }
+
+    const float altitudeD = verticalVelocityCmS * dBoost * altitudePidCoeffs.Kd;
 
     const float altitudeF = targetAltitudeStep * altitudePidCoeffs.Kf;
 
-    const float hoverOffset = autopilotConfig()->hover_throttle - PWM_RANGE_MIN;
-
+    const float hoverOffset = apConfig()->hover_throttle - PWM_RANGE_MIN;
     float throttleOffset = altitudeP + altitudeI - altitudeD + altitudeF + hoverOffset;
-    const float tiltMultiplier = 2.0f - fmaxf(getCosTiltAngle(), 0.5f);
-    // 1 = flat, 1.24 at 40 degrees, max 1.5 around 60 degrees, the default limit of Angle Mode
-    // 2 - cos(x) is between 1/cos(x) and 1/sqrt(cos(x)) in this range
+
+    const float tiltMultiplier = 1.0f / fmaxf(getCosTiltAngle(), 0.5f);
+    // 1 = flat, 1.3 at 40 degrees, 1.56 at 50 deg, max 2.0 at 60 degrees or higher
+    // note: the default limit of Angle Mode is 60 degrees
+
     throttleOffset *= tiltMultiplier;
 
     float newThrottle = PWM_RANGE_MIN + throttleOffset;
-    newThrottle = constrainf(newThrottle, autopilotConfig()->throttle_min, autopilotConfig()->throttle_max);
+    newThrottle = constrainf(newThrottle, apConfig()->throttle_min, apConfig()->throttle_max);
     DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 0, lrintf(newThrottle)); // normal range 1000-2000 but is before constraint
 
     newThrottle = scaleRangef(newThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
@@ -91,17 +215,183 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float vertical
     DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 7, lrintf(altitudeF));
 }
 
-bool isBelowLandingAltitude(void)
+void setSticksActiveStatus(bool areSticksActive)
 {
-    return getAltitudeCm() < 100.0f * autopilotConfig()->landing_altitude_m;
+    ap.sticksActive = areSticksActive;
 }
 
-const pidCoefficient_t *getAltitudePidCoeffs(void)
+void setTargetLocationByAxis(const gpsLocation_t* newTargetLocation, axisEF_e efAxisIdx)
+// not used at present but needed by upcoming GPS code
 {
-    return &altitudePidCoeffs;
+    if (efAxisIdx == LON) {
+        ap.targetLocation.lon = newTargetLocation->lon; // update East-West / / longitude position
+    } else {
+        ap.targetLocation.lat = newTargetLocation->lat; // update North-South / latitude position
+    }
+}
+
+bool positionControl(void)
+{
+    unsigned debugAxis = gyroConfig()->gyro_filter_debug_axis;
+    static vector2_t debugGpsDistance = { 0 };     // keep last calculated distance for DEBUG
+    static vector2_t debugPidSumEF = { 0 };        // and last pidsum in EF
+    static uint16_t gpsStamp = 0;
+    if (gpsHasNewData(&gpsStamp)) {
+        const float gpsDataInterval = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.05 - 2.5s
+        const float gpsDataFreq = getGpsDataFrequencyHz();
+
+        // get lat and long distances from current location (gpsSol.llh) to target location
+        vector2_t gpsDistance;
+        GPS_distance2d(&gpsSol.llh, &ap.targetLocation, &gpsDistance); // X is EW/lon, Y is NS/lat
+        debugGpsDistance = gpsDistance;
+        const float distanceNormCm = vector2Norm(&gpsDistance);
+
+        // ** Sanity check **
+        // primarily to detect flyaway from no Mag or badly oriented Mag
+        // must accept some overshoot at the start, especially if entering at high speed
+        if (distanceNormCm > ap.sanityCheckDistance) {
+            return false;
+        }
+
+        // update filters according to current GPS update rate
+        const float vaGain = pt1FilterGain(ap.vaLpfCutoff, gpsDataInterval);
+        const float iTermLeakGain = 1.0f - pt1FilterGainFromDelay(2.5f, gpsDataInterval);   // 2.5s time constant
+        vector2_t pidSum = { 0 };       // P+I in loop, D+A added after the axis loop (after limiting it)
+        vector2_t pidDA;                // D+A
+
+        for (axisEF_e efAxisIdx = LON; efAxisIdx <= LAT; efAxisIdx++) {
+            efPidAxis_t *efAxis = &ap.efAxis[efAxisIdx];
+            // separate PID controllers for longitude (EastWest or EW, X) and latitude (NorthSouth or NS, Y)
+            const float axisDistance = gpsDistance.v[efAxisIdx];
+
+            // ** P **
+            const float pidP = axisDistance * positionPidCoeffs.Kp;
+            pidSum.v[efAxisIdx] += pidP;
+
+            // ** I **
+            // only add to iTerm while in hold phase
+            efAxis->integral += efAxis->isStopping ? 0.0f : axisDistance * gpsDataInterval;
+            const float pidI = efAxis->integral * positionPidCoeffs.Ki;
+            pidSum.v[efAxisIdx] += pidI;
+
+            // ** D ** //
+            // Velocity derived from GPS position works better than module supplied GPS Speed and Heading information
+
+            const float velocity = (axisDistance - efAxis->previousDistance) * gpsDataFreq; // cm/s
+            efAxis->previousDistance = axisDistance;
+            pt1FilterUpdateCutoff(&efAxis->velocityLpf, vaGain);
+            const float velocityFiltered = pt1FilterApply(&efAxis->velocityLpf, velocity);
+            float pidD = velocityFiltered * positionPidCoeffs.Kd;
+
+            // differentiate velocity another time to get acceleration
+            float acceleration = (velocityFiltered - efAxis->previousVelocity) * gpsDataFreq;
+            efAxis->previousVelocity = velocityFiltered;
+            // apply second filter to acceleration (acc is filtered twice)
+            pt1FilterUpdateCutoff(&efAxis->accelerationLpf, vaGain);
+            const float accelerationFiltered = pt1FilterApply(&efAxis->accelerationLpf, acceleration);
+            const float pidA = accelerationFiltered * positionPidCoeffs.Kf;
+
+            if (ap.sticksActive) {
+                // sticks active 'phase', prepare to enter stopping
+                efAxis->isStopping = true;
+                // slowly leak iTerm away
+                efAxis->integral *= iTermLeakGain;
+                efAxis->previousDistance = 0.0f; // avoid D and A spikes
+                // rest is handled after axis loop
+            } else if (efAxis->isStopping) {
+                // 'phase' after sticks are centered, but before craft has stopped; in given Earth axis
+                pidD *= 1.6f; // aribitrary D boost to stop more quickly than usual
+                // detect when axis has nearly stopped by sign reversal of velocity (comparing sign of velocityFiltered, which is delayed, to velocity)
+                if (velocity * velocityFiltered < 0.0f) {
+                    setTargetLocationByAxis(&gpsSol.llh, efAxisIdx);  // reset target location for this axis, forcing P to zero
+                    efAxis->previousDistance = 0.0f;                  // ensure minimal D jump from the updated location
+                    efAxis->isStopping = false;                       // end the 'stopping' phase
+                    if (ap.efAxis[LAT].isStopping == ap.efAxis[LON].isStopping) {
+                        // when both axes have stopped moving, reset the sanity distance to 10m default
+                        ap.sanityCheckDistance = sanityCheckDistance(1000);
+                    }
+                }
+            }
+            pidDA.v[efAxisIdx] = pidD + pidA;    // save DA here, processed after axis loop
+            if (debugAxis == efAxisIdx) {
+                DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceNormCm));   // same for both axes
+                DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10));
+                DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(pidI * 10));
+                DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidD * 10));
+                DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pidA * 10));
+            }
+        } // end for loop
+
+        {
+            // limit sum of D and A per axis based on total DA vector length, otherwise can be too aggressive when starting at speed
+            // limit is 35 degrees from D and A alone, arbitrary value.  20 is a bit too low, allows a lot of overshoot
+            // note: an angle of more than 35 degrees can still be achieved as P and I grow
+            const float maxDAAngle = 35.0f; // D+A limit in degrees; arbitrary angle
+            const float mag = vector2Norm(&pidDA);
+            if (mag > maxDAAngle) {
+                vector2Scale(&pidDA, &pidDA, maxDAAngle / mag);
+            }
+        }
+
+        // add constrained DA to sum
+        vector2Add(&pidSum, &pidSum, &pidDA);
+        debugPidSumEF = pidSum;
+        vector2_t anglesBF;
+
+        if (ap.sticksActive) {
+            // if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
+            anglesBF = (vector2_t){{0, 0}};             // set output PIDS to 0; upsampling filter will smooth this
+            // reset target location each cycle (and set previousDistance to zero in for loop), to keep D current, and avoid a spike when stopping
+            ap.targetLocation = gpsSol.llh;
+            // keep updating sanity check distance while sticks are out because speed may get high
+            ap.sanityCheckDistance = sanityCheckDistance(gpsSol.groundSpeed);
+        } else {
+            // ** Rotate pid Sum to body frame, and convert it into pitch and roll **
+            // attitude.values.yaw increases clockwise from north
+            // PID is running in ENU, adapt angle (to 0deg = EAST);
+            //  rotation is from EarthFrame to BodyFrame, no change of sign from heading
+            const float angle = DECIDEGREES_TO_RADIANS(attitude.values.yaw - 900);
+            vector2_t pidBodyFrame;   // pid output in body frame; X is forward, Y is left
+            vector2Rotate(&pidBodyFrame, &pidSum, angle);         // rotate by angle counterclockwise
+            anglesBF.v[AI_ROLL] = -pidBodyFrame.y;         // negative roll to fly left
+            anglesBF.v[AI_PITCH] = pidBodyFrame.x;         // positive pitch for forward
+             // limit angle vector to maxAngle
+            const float mag = vector2Norm(&anglesBF);
+            if (mag > ap.maxAngle && mag > 0.0f) {
+                vector2Scale(&anglesBF, &anglesBF, ap.maxAngle / mag);
+            }
+        }
+        ap.pidSumBF = anglesBF;    // this value will be upsampled
+    }
+
+    // Final output to pid.c Angle Mode at 100Hz with PT3 upsampling
+    for (unsigned i = 0; i < RP_AXIS_COUNT; i++) {
+        // note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll
+        autopilotAngle[i] = pt3FilterApply(&ap.upsampleLpfBF[i], ap.pidSumBF.v[i]);
+    }
+
+    if (debugAxis < 2) {
+        // this is different from @ctzsnooze version
+        // debugAxis = 0: store longitude + roll
+        // debugAxis = 1: store latitude + pitch
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(debugGpsDistance.v[debugAxis]));    // cm
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(debugPidSumEF.v[debugAxis] * 10));  // deg * 10
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[debugAxis] * 10));   // deg * 10
+    }
+    return true;
+}
+
+bool isBelowLandingAltitude(void)
+{
+    return getAltitudeCm() < 100.0f * apConfig()->landing_altitude_m;
 }
 
 float getAutopilotThrottle(void)
 {
     return throttleOut;
 }
+
+bool isAutopilotInControl(void)
+{
+    return !ap.sticksActive;
+}

src/main/flight/autopilot.h

@@ -17,14 +17,18 @@
 
 #pragma once
 
-#include "pg/autopilot.h"
-#include "flight/pid.h"
+#include "io/gps.h"
 
-void autopilotInit(const autopilotConfig_t *config);
+extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
+
+void autopilotInit(void);
 void resetAltitudeControl(void);
-
-void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
+void setSticksActiveStatus(bool areSticksActive);
+void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz);
+void posControlOutput(void);
+bool positionControl(void);
+void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep);
 
 bool isBelowLandingAltitude(void);
-const pidCoefficient_t *getAltitudePidCoeffs(void);
 float getAutopilotThrottle(void);
+bool isAutopilotInControl(void);

src/main/flight/gps_rescue.c

@@ -46,6 +46,7 @@
 
 #include "io/gps.h"
 #include "rx/rx.h"
+#include "pg/autopilot.h"
 #include "sensors/acceleration.h"
 
 #include "gps_rescue.h"
@@ -83,7 +84,6 @@ typedef struct {
     float rollAngleLimitDeg;
     float descentDistanceM;
     int8_t secondsFailing;
-    float throttleDMultiplier;
     float yawAttenuator;
     float disarmThreshold;
     float velocityITermAccumulator;
@@ -121,14 +121,13 @@ typedef struct {
 #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 };
+float gpsRescueAngle[RP_AXIS_COUNT] = { 0, 0 };
 bool magForceDisable = false;
-static bool newGPSData = false;
 static pt1Filter_t velocityDLpf;
 static pt3Filter_t velocityUpsampleLpf;
 
+
 rescueState_s rescueState;
 
 void gpsRescueInit(void)
@@ -139,20 +138,11 @@ void gpsRescueInit(void)
     rescueState.intent.velocityPidCutoffModifier = 1.0f;
     gain = pt1FilterGain(cutoffHz, 1.0f);
     pt1FilterInit(&velocityDLpf, gain);
-
-    cutoffHz *= 4.0f;
+    cutoffHz *= 4.0f; 
     gain = pt3FilterGain(cutoffHz, taskIntervalSeconds);
     pt3FilterInit(&velocityUpsampleLpf, gain);
 }
 
-/*
- If we have new GPS data, update home heading if possible and applicable.
-*/
-void gpsRescueNewGpsData(void)
-{
-    newGPSData = true;
-}
-
 static void rescueStart(void)
 {
     rescueState.phase = RESCUE_INITIALIZE;
@@ -164,7 +154,7 @@ static void rescueStop(void)
 }
 
 // Things that need to run when GPS Rescue is enabled, and while armed, but while there is no Rescue in place
-static void setReturnAltitude(void)
+static void setReturnAltitude(bool newGpsData)
 {
     // Hold maxAltitude at zero while disarmed, but if set_home_point_once is true, hold maxAlt until power cycled
     if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) {
@@ -175,7 +165,7 @@ static void setReturnAltitude(void)
     // While armed, but not during the rescue, update the max altitude value
     rescueState.intent.maxAltitudeCm = fmaxf(getAltitudeCm(), rescueState.intent.maxAltitudeCm);
 
-    if (newGPSData) {
+    if (newGpsData) {
         // set the target altitude to the current altitude.
         rescueState.intent.targetAltitudeCm = getAltitudeCm();
 
@@ -200,7 +190,7 @@ static void setReturnAltitude(void)
     }
 }
 
-static void rescueAttainPosition(void)
+static void rescueAttainPosition(bool newGpsData)
 {
     // runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
     static float previousVelocityError = 0.0f;
@@ -210,7 +200,6 @@ static void rescueAttainPosition(void)
         // values to be returned when no rescue is active
         gpsRescueAngle[AI_PITCH] = 0.0f;
         gpsRescueAngle[AI_ROLL] = 0.0f;
-        rescueThrottle = rcCommand[THROTTLE];
         return;
     case RESCUE_INITIALIZE:
         // Initialize internal variables each time GPS Rescue is started
@@ -221,10 +210,9 @@ static void rescueAttainPosition(void)
         rescueState.sensor.imuYawCogGain = 1.0f;
         return;
     case RESCUE_DO_NOTHING:
-        // 20s of slow descent for switch induced sanity failures to allow time to recover
+        // 20s of hover for switch induced sanity failures to allow time to recover
         gpsRescueAngle[AI_PITCH] = 0.0f;
         gpsRescueAngle[AI_ROLL] = 0.0f;
-        rescueThrottle = autopilotConfig()->hover_throttle - 100;
         return;
      default:
         break;
@@ -233,9 +221,7 @@ static void rescueAttainPosition(void)
     /**
         Altitude (throttle) controller
     */
-
-    const float verticalVelocity = getAltitudeDerivative() * rescueState.intent.throttleDMultiplier;
-    altitudeControl(rescueState.intent.targetAltitudeCm, taskIntervalSeconds, verticalVelocity, rescueState.intent.targetAltitudeStepCm);
+    altitudeControl(rescueState.intent.targetAltitudeCm, taskIntervalSeconds, rescueState.intent.targetAltitudeStepCm);
 
     /**
         Heading / yaw controller
@@ -277,7 +263,7 @@ static void rescueAttainPosition(void)
         Pitch / velocity controller
     */
     static float pitchAdjustment = 0.0f;
-    if (newGPSData) {
+    if (newGpsData) {
 
         const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
 
@@ -488,7 +474,7 @@ static void performSanityChecks(void)
     DEBUG_SET(DEBUG_RTH, 3, (rescueState.intent.secondsFailing * 100 + secondsLowSats));
 }
 
-static void sensorUpdate(void)
+static void sensorUpdate(bool newGpsData)
 {
     static float prevDistanceToHomeCm = 0.0f;
 
@@ -513,7 +499,7 @@ static void sensorUpdate(void)
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 4, lrintf(attitude.values.yaw));                 // estimated heading of the quad (direction nose is pointing in)
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 5, lrintf(rescueState.sensor.directionToHome));  // angle to home derived from GPS location and home position
 
-    if (!newGPSData) {
+    if (!newGpsData) {
         return;
         // GPS ground speed, velocity and distance to home will be held at last good values if no new packets
     }
@@ -523,9 +509,9 @@ static void sensorUpdate(void)
     rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
 
     rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
-    // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
+    // Range from 50ms (20hz) to 2500ms (0.4Hz). Intended to cover common GPS data rates and exclude unusual values.
 
-    rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds);
+    rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) * getGpsDataFrequencyHz());
     // positive = towards home.  First value is useless since prevDistanceToHomeCm was zero.
     prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
 
@@ -639,11 +625,11 @@ void disarmOnImpact(void)
     }
 }
 
-void descend(void)
+void descend(bool newGpsData)
 {
-    if (newGPSData) {
+    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 * autopilotConfig()->landing_altitude_m);
+        const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * apConfig()->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
@@ -678,22 +664,11 @@ void descend(void)
     // set the altitude step, considering the interval between altitude readings and the descent rate
     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);
-    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);
-    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;
+    // at or below 10m: descend at 0.6x set value; above 10m, descend faster, to max 3.0x at 50m
+    altitudeStepCm *= scaleRangef(constrainf(rescueState.intent.targetAltitudeCm, 1000, 5000), 1000, 5000, 0.6f, 3.0f);
 
     rescueState.intent.targetAltitudeStepCm = -altitudeStepCm;
-    rescueState.intent.targetAltitudeCm += rescueState.intent.targetAltitudeStepCm;
+    rescueState.intent.targetAltitudeCm -= altitudeStepCm;
 }
 
 void initialiseRescueValues (void)
@@ -702,7 +677,6 @@ void initialiseRescueValues (void)
     rescueState.intent.yawAttenuator = 0.0f; // no yaw in the climb
     rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS; // avoid snap from D at the start
     rescueState.intent.rollAngleLimitDeg = 0.0f; // no roll until flying home
-    rescueState.intent.throttleDMultiplier = 1.0f;
     rescueState.intent.velocityPidCutoffModifier = 1.0f; // normal velocity lowpass filter cutoff
     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
@@ -713,17 +687,19 @@ void initialiseRescueValues (void)
 void gpsRescueUpdate(void)
 // runs at gpsRescueTaskIntervalSeconds, and runs whether or not rescue is active
 {
+    static uint16_t gpsStamp = 0;
+    bool newGpsData = gpsHasNewData(&gpsStamp);
+
     if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
         rescueStop(); // sets phase to RESCUE_IDLE; does nothing else.  RESCUE_IDLE tasks still run.
     } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) {
         rescueStart(); // sets phase to rescue_initialise if we enter GPS Rescue mode while idle
-        rescueAttainPosition(); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably)
+        rescueAttainPosition(false); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably)
         performSanityChecks(); // Initialises sanity check values when a Rescue starts
     }
-
     // Will now be in RESCUE_INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE
 
-    sensorUpdate(); // always get latest GPS and Altitude data, update ascend and descend rates
+    sensorUpdate(newGpsData); // always get latest GPS and Altitude data, update ascend and descend rates
 
     static bool returnAltitudeLow = true;
     static bool initialVelocityLow = true;
@@ -733,7 +709,7 @@ void gpsRescueUpdate(void)
     case RESCUE_IDLE:
         // in Idle phase = NOT in GPS Rescue
         // update the return altitude and descent distance values, to have valid settings immediately they are needed
-        setReturnAltitude();
+        setReturnAltitude(newGpsData);
         break;
         // sanity checks are bypassed in IDLE mode; instead, failure state is always initialised to HEALTHY
         // target altitude is always set to current altitude.
@@ -824,7 +800,7 @@ void gpsRescueUpdate(void)
         rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax;
         // higher velocity filter cutoff for initial few seconds to improve accuracy; can be smoother later
 
-        if (newGPSData) {
+        if (newGpsData) {
             // cut back on allowed angle if there is a high groundspeed error
             rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle;
             // introduce roll slowly and limit to half the max pitch angle; earth referenced yaw may add more roll via angle code
@@ -843,14 +819,14 @@ void gpsRescueUpdate(void)
             rescueState.phase = RESCUE_LANDING;
             rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
         }
-        descend();
+        descend(newGpsData);
         break;
 
     case RESCUE_LANDING:
         // Reduce altitude target steadily until impact, then disarm.
         // control yaw angle and throttle and pitch, attenuate velocity, roll and pitch iTerm
         // increase velocity smoothing cutoff as we get closer to ground
-        descend();
+        descend(newGpsData);
         disarmOnImpact();
         break;
 
@@ -873,19 +849,17 @@ void gpsRescueUpdate(void)
     DEBUG_SET(DEBUG_RTH, 0, lrintf(rescueState.intent.maxAltitudeCm / 10.0f));
 
     performSanityChecks();
-    rescueAttainPosition();
-
-    newGPSData = false;
+    rescueAttainPosition(newGpsData);
 }
 
 float gpsRescueGetYawRate(void)
 {
-    return rescueYaw;
+    return rescueYaw; // the control yaw value for rc.c to be used while flightMode gps_rescue is active.
 }
 
 float gpsRescueGetImuYawCogGain(void)
 {
-    return rescueState.sensor.imuYawCogGain;
+    return rescueState.sensor.imuYawCogGain; // to speed up the IMU orientation to COG when needed
 }
 
 bool gpsRescueIsConfigured(void)
@@ -895,11 +869,11 @@ bool gpsRescueIsConfigured(void)
 
 bool gpsRescueIsAvailable(void)
 {
-    return rescueState.isAvailable;
+    return rescueState.isAvailable; // flashes the warning when not available (low sats, etc)
 }
 
 bool gpsRescueIsDisabled(void)
-// used for OSD warning
+// used for OSD warning, needs review
 {
     return (!STATE(GPS_FIX_HOME));
 }

src/main/flight/gps_rescue.h

@@ -45,11 +45,10 @@ typedef enum {
     GPS_RESCUE_ALT_MODE_COUNT
 } gpsRescueAltitudeMode_e;
 
-extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
+extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
 void gpsRescueInit(void);
 void gpsRescueUpdate(void);
-void gpsRescueNewGpsData(void);
 float gpsRescueGetYawRate(void);
 bool gpsRescueIsConfigured(void);
 bool gpsRescueIsAvailable(void);

src/main/flight/imu.c

@@ -85,7 +85,8 @@ static bool imuUpdated = false;
 #define ATTITUDE_RESET_GYRO_LIMIT 15       // 15 deg/sec - gyro limit for quiet period
 #define ATTITUDE_RESET_ACTIVE_TIME 500000  // 500ms - Time to wait for attitude to converge at high gain
 #define GPS_COG_MIN_GROUNDSPEED 100        // 1.0m/s - min groundspeed for GPS Heading reinitialisation etc
-bool canUseGPSHeading = true;
+
+bool canUseGPSHeading = false;
 
 static float throttleAngleScale;
 static int throttleAngleValue;
@@ -187,12 +188,7 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
 
 void imuInit(void)
 {
-#ifdef USE_GPS
-    canUseGPSHeading = true;
-#else
     canUseGPSHeading = false;
-#endif
-
     imuComputeRotationMatrix();
 
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
@@ -441,7 +437,8 @@ static float imuCalcGroundspeedGain(float dt)
     }
 
     // NOTE : these suppressions make sense with normal pilot inputs and normal flight
-    // They are not used in GPS Rescue, and probably should be bypassed in position hold, etc,
+    // Flying straight ahead for 1s at > 3m/s at pitch of say 22.5 degrees returns a final multiplier of 5
+    // They are not used in GPS Rescue, and probably should be bypassed in position hold, and other situations when we know we are still
 
     return speedBasedGain * stickSuppression * rollSuppression * pitchSuppression;
 }
@@ -611,6 +608,28 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 }
 #else
 
+#if defined(USE_GPS)
+static void updateGpsHeadingUsable(float groundspeedGain, float imuCourseError, float dt)
+{
+    if (!canUseGPSHeading) {
+        static float gpsHeadingConfidence = 0;
+        // groundspeedGain can be 5.0 in clean forward flight, up to 10.0 max
+        // fabsf(imuCourseError) is 0 when headings are aligned, 1 when 90 degrees error or worse
+        // accumulate 'points' based on alignment and likelihood of accumulation being good
+        gpsHeadingConfidence += fmaxf(groundspeedGain - fabsf(imuCourseError), 0.0f) * dt;
+        // recenter at 2.5s time constant
+        // TODO: intent is to match IMU time constant, approximately, but I don't exactly know how to do that
+        gpsHeadingConfidence -= 0.4 * dt * gpsHeadingConfidence; 
+        // if we accumulate enough 'points' over time, the IMU probably is OK
+        // will need to reaccumulate after a disarm (will be retained partly for very brief disarms)
+        canUseGPSHeading = gpsHeadingConfidence > 2.0f;
+        // canUseGPSHeading blocks position hold until suitable GPS heading, when GPS is the only heading source
+        // NOTE: I think that this check only runs once after power up
+        // If the GPS heading is lost on disarming, then it will need to be reset each disarm
+    }
+}
+#endif
+
 static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 {
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
@@ -662,9 +681,16 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
                 // 0.0 - 10.0, heuristic based on GPS speed and stick state
                 groundspeedGain = imuCalcGroundspeedGain(dt);
             }
+
             DEBUG_SET(DEBUG_ATTITUDE, 2, lrintf(groundspeedGain * 100.0f));
-            float courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-            cogErr = imuCalcCourseErr(courseOverGround) * groundspeedGain;
+
+            const float courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+            const float imuCourseError = imuCalcCourseErr(courseOverGround);
+            cogErr = imuCourseError * groundspeedGain;
+            // cogErr is greater with larger heading errors and greater speed in straight pitch forward flight
+
+            updateGpsHeadingUsable(groundspeedGain, imuCourseError, dt);
+
         } else if (gpsSol.groundSpeed > GPS_COG_MIN_GROUNDSPEED) {
             // Reset the reference and reinitialize quaternion factors when GPS groundspeed > GPS_COG_MIN_GROUNDSPEED
             imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);

src/main/flight/imu.h

@@ -46,7 +46,7 @@ typedef struct {
 typedef union {
     int16_t raw[XYZ_AXIS_COUNT];
     struct {
-        // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
+        // absolute angle inclination in multiple of 0.1 degree  eg attitude.values.yaw 180 deg = 1800
         int16_t roll;
         int16_t pitch;
         int16_t yaw;

src/main/flight/mixer.c

@@ -835,8 +835,8 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
     if (featureIsEnabled(FEATURE_MOTOR_STOP)
         && ARMING_FLAG(ARMED)
         && !mixerRuntime.feature3dEnabled
-        && !airmodeEnabled                               // not with airmode or launch control
-        && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE) // not in autopilot modes
+        && !airmodeEnabled
+        && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE)   // disable motor_stop while GPS Rescue / Alt Hold / Pos Hold is active
         && (rcData[THROTTLE] < rxConfig()->mincheck)) {
         applyMotorStop();
     } else {

src/main/flight/pid.c

@@ -46,7 +46,6 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/alt_hold.h"
 #include "flight/autopilot.h"
 #include "flight/gps_rescue.h"
 #include "flight/imu.h"
@@ -58,6 +57,8 @@
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
+#include "pg/autopilot.h"
+
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/gyro.h"
@@ -545,7 +546,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
 {
     // Applies only to axes that are in Angle mode
     // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
-    const float angleLimit = pidProfile->angle_limit;
+    float angleLimit = pidProfile->angle_limit;
     float angleFeedforward = 0.0f;
     // if user changes rates profile, update the max setpoint for angle mode
     const float maxSetpointRateInv = 1.0f / getMaxRcRate(axis);
@@ -569,6 +570,21 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
 #ifdef USE_GPS_RESCUE
     angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
 #endif
+#ifdef USE_POS_HOLD_MODE
+    if (FLIGHT_MODE(POS_HOLD_MODE)) {
+        angleFeedforward = 0.0f; // otherwise the lag of the PT3 carries recent stick inputs into the hold
+        if (isAutopilotInControl()) {
+            // sticks are not deflected
+            angleTarget = autopilotAngle[axis]; // autopilotAngle in degrees
+            angleLimit = 85.0f; // allow autopilot to use whatever angle it needs to stop
+        }
+        // limit pilot requested angle to half the autopilot angle to avoid excess speed and chaotic stops
+        angleLimit = fminf(0.5f * apConfig()->max_angle, angleLimit);
+    }
+#endif
+
+    angleTarget = constrainf(angleTarget, -angleLimit, angleLimit);
+
     const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; // stepped at 500hz with some 4ms flat spots
     const float errorAngle = angleTarget - currentAngle;
     float angleRate = errorAngle * pidRuntime.angleGain + angleFeedforward;
@@ -586,7 +602,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
     // this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy
     angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate);
 
-    if (FLIGHT_MODE(ANGLE_MODE| GPS_RESCUE_MODE)) {
+    if (FLIGHT_MODE(ANGLE_MODE| GPS_RESCUE_MODE | POS_HOLD_MODE)) {
         currentPidSetpoint = angleRate;
     } else {
         // can only be HORIZON mode - crossfade Angle rate and Acro rate
@@ -707,7 +723,7 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
 {
     float ret = setPoint;
 
-    if (!FLIGHT_MODE(ANGLE_MODE  | HORIZON_MODE | GPS_RESCUE_MODE | ALT_HOLD_MODE)) {
+    if (!FLIGHT_MODE(ANGLE_MODE  | HORIZON_MODE | GPS_RESCUE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE)) {
         bool resetIterm = false;
         float projectedAngle = 0;
         const int setpointSign = acroTrainerSign(setPoint);
@@ -926,10 +942,8 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
     if (wasThrottleRaised()
         // and, either sticks are centred and throttle zeroed,
         && ((getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f)
-            // we could test here for stage 2 failsafe (including both landing or GPS Rescue)
-            // this may permit removing the GPS Rescue disarm method altogether
 #ifdef USE_ALT_HOLD_MODE
-            // or in altitude hold mode, including failsafe landing mode, indirectly
+            // or, in altitude hold mode, where throttle can be non-zero
             || FLIGHT_MODE(ALT_HOLD_MODE)
 #endif
         )) {
@@ -1110,7 +1124,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
     const bool isExternalAngleModeRequest = FLIGHT_MODE(GPS_RESCUE_MODE)
 #ifdef USE_ALT_HOLD_MODE
-                || FLIGHT_MODE(ALT_HOLD_MODE)
+                || FLIGHT_MODE(ALT_HOLD_MODE) // todo - check if this is needed
+#endif
+#ifdef USE_POS_HOLD_MODE
+                || FLIGHT_MODE(POS_HOLD_MODE) 
 #endif
                 ;
     levelMode_e levelMode;

src/main/flight/pid.h

@@ -462,7 +462,7 @@ typedef struct pidRuntime_s {
     uint8_t acroTrainerDebugAxis;
     float acroTrainerGain;
     bool acroTrainerActive;
-    int acroTrainerAxisState[2];  // only need roll and pitch
+    int acroTrainerAxisState[RP_AXIS_COUNT];  // only need roll and pitch
 #endif
 
 #ifdef USE_DYN_LPF
@@ -508,12 +508,12 @@ typedef struct pidRuntime_s {
 #endif
 
 #ifdef USE_ACC
-    pt3Filter_t attitudeFilter[2];  // Only for ROLL and PITCH
+    pt3Filter_t attitudeFilter[RP_AXIS_COUNT];  // Only for ROLL and PITCH
     pt1Filter_t horizonSmoothingPt1;
     uint16_t horizonDelayMs;
     float angleYawSetpoint;
     float angleEarthRef;
-    float angleTarget[2];
+    float angleTarget[RP_AXIS_COUNT];
     bool axisInAngleMode[3];
 #endif
 

src/main/flight/pos_hold.c

@@ -0,0 +1,106 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_POS_HOLD_MODE
+
+#include "math.h"
+#include "build/debug.h"
+#include "common/maths.h"
+
+#include "config/config.h"
+#include "fc/core.h"
+#include "fc/runtime_config.h"
+#include "fc/rc.h"
+#include "flight/autopilot.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/position.h"
+#include "rx/rx.h"
+#include "sensors/compass.h"
+
+#include "pg/pos_hold.h"
+#include "pos_hold.h"
+
+typedef struct posHoldState_s {
+    bool isEnabled;
+    bool isControlOk;
+    bool areSensorsOk;
+    float deadband;
+    bool useStickAdjustment;
+} posHoldState_t;
+
+static posHoldState_t posHold;
+
+void posHoldInit(void)
+{
+    posHold.deadband = posHoldConfig()->pos_hold_deadband * 0.01f;
+    posHold.useStickAdjustment = posHoldConfig()->pos_hold_deadband;
+}
+
+void posHoldCheckSticks(void)
+{
+    // if failsafe is active, eg landing mode, don't update the original start point 
+    if (!failsafeIsActive() && posHold.useStickAdjustment) {
+        const bool sticksDeflected = (getRcDeflectionAbs(FD_ROLL) > posHold.deadband) || (getRcDeflectionAbs(FD_PITCH) > posHold.deadband);
+        setSticksActiveStatus(sticksDeflected);
+    }
+}
+
+bool sensorsOk(void)
+{
+    if (!STATE(GPS_FIX)) {
+        return false; 
+    }
+    if (
+#ifdef USE_MAG
+        !compassIsHealthy() &&
+#endif
+        (!posHoldConfig()->pos_hold_without_mag || !canUseGPSHeading)) {
+        return false; 
+    }
+    return true;
+}
+
+void updatePosHold(timeUs_t currentTimeUs) {
+    UNUSED(currentTimeUs); 
+    if (FLIGHT_MODE(POS_HOLD_MODE)) {
+        if (!posHold.isEnabled) {
+            resetPositionControl(&gpsSol.llh, POSHOLD_TASK_RATE_HZ); // sets target location to current location
+            posHold.isControlOk = true;
+            posHold.isEnabled = true;
+        }
+    } else {
+        posHold.isEnabled = false;
+    }
+
+    if (posHold.isEnabled && posHold.isControlOk) {
+        posHold.areSensorsOk = sensorsOk();
+        if (posHold.areSensorsOk) {
+            posHoldCheckSticks();
+            posHold.isControlOk = positionControl(); // false only on sanity check failure
+        }
+    }
+}
+
+bool posHoldFailure(void) {
+    // used only to display warning in OSD if requested but failing
+    return FLIGHT_MODE(POS_HOLD_MODE) && (!posHold.isControlOk || !posHold.areSensorsOk);
+}
+
+#endif // USE_POS_HOLD

src/main/flight/pos_hold.h

@@ -0,0 +1,33 @@
+/*
+ * 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/pos_hold.h"
+
+#ifdef USE_POS_HOLD_MODE
+#include "common/time.h"
+#include "io/gps.h"
+
+#define POSHOLD_TASK_RATE_HZ 100 // hz
+
+void posHoldInit(void);
+void updatePosHold(timeUs_t currentTimeUs);
+
+bool posHoldFailure(void);
+
+#endif

src/main/io/gps.c

@@ -53,9 +53,9 @@
 #include "fc/gps_lap_timer.h"
 #include "fc/runtime_config.h"
 
+#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
-#include "flight/gps_rescue.h"
 
 #include "scheduler/scheduler.h"
 
@@ -92,7 +92,10 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
 #define GPS_TASK_DECAY_SHIFT 9         // Smoothing factor for GPS task re-scheduler
 
 static serialPort_t *gpsPort;
-static float gpsDataIntervalSeconds;
+static float gpsDataIntervalSeconds = 0.1f;
+static float gpsDataFrequencyHz = 10.0f;
+
+static uint16_t currentGpsStamp = 0; // logical timer for received position update
 
 typedef struct gpsInitData_s {
     uint8_t index;
@@ -1764,7 +1767,7 @@ static bool writeGpsSolutionNmea(gpsSolutionData_t *sol, const gpsDataNmea_t *da
             }
              navDeltaTimeMs = (msInTenSeconds + data->time - gpsData.lastNavSolTs) % msInTenSeconds;
              gpsData.lastNavSolTs = data->time;
-             sol->navIntervalMs = constrain(navDeltaTimeMs, 100, 2500);
+             sol->navIntervalMs = constrain(navDeltaTimeMs, 50, 2500);
             // return only one true statement to trigger one "newGpsDataReady" flag per GPS loop
             return true;
 
@@ -2182,7 +2185,7 @@ static bool UBLOX_parse_gps(void)
         *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_VELNED;
 #endif
         gpsSol.speed3d = ubxRcvMsgPayload.ubxNavVelned.speed_3d;       // cm/s
-        gpsSol.groundSpeed = ubxRcvMsgPayload.ubxNavVelned.speed_2d;    // cm/s
+        gpsSol.groundSpeed = ubxRcvMsgPayload.ubxNavVelned.speed_2d;   // cm/s
         gpsSol.groundCourse = (uint16_t) (ubxRcvMsgPayload.ubxNavVelned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
         ubxHaveNewSpeed = true;
         break;
@@ -2512,7 +2515,7 @@ void GPS_calc_longitude_scaling(int32_t lat)
 //
 static void GPS_calculateDistanceFlown(bool initialize)
 {
-    static gpsLocation_t lastLLH = {0, 0, 0};
+    static gpsLocation_t lastLLH = {0};
 
     if (initialize) {
         GPS_distanceFlownInCm = 0;
@@ -2556,11 +2559,12 @@ void GPS_reset_home_position(void)
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
-#define EARTH_ANGLE_TO_CM (111.3195f * 1000 * 100 / GPS_DEGREES_DIVIDER)  // latitude unit to cm at equator (111km/deg)
-// Get distance between two points in cm
-// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
+// Get distance between two points in cm using spherical to Cartesian transform
+// One one latitude unit, or one longitude unit at the equator, equals 1.113195 cm.
+// Get bearing from pos1 to pos2, returns values with 0.01 degree precision
 void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t* to, bool dist3d, uint32_t *pDist, int32_t *pBearing)
 {
+    // TO DO : handle crossing the 180 degree meridian, as in `GPS_distance2d()`
     float dLat = (to->lat - from->lat) * EARTH_ANGLE_TO_CM;
     float dLon = (to->lon - from->lon) * GPS_cosLat * EARTH_ANGLE_TO_CM; // convert to local angle
     float dAlt = dist3d ? to->altCm - from->altCm : 0;
@@ -2593,6 +2597,24 @@ void GPS_calculateDistanceAndDirectionToHome(void)
     }
 }
 
+// return distance vector in local, cartesian ENU coordinates
+// note that parameter order is from, to
+void GPS_distance2d(const gpsLocation_t *from, const gpsLocation_t *to, vector2_t *distance)
+{
+    int32_t deltaLon = to->lon - from->lon;
+    // In case we crossed the 180° meridian:
+    const int32_t deg180 = 180 * GPS_DEGREES_DIVIDER; // number of integer longitude steps in 180 degrees
+    if (deltaLon > deg180) {
+        deltaLon -= deg180;  // 360 * GPS_DEGREES_DIVIDER overflows int32_t, so use 180 twice
+        deltaLon -= deg180;
+    } else if (deltaLon <= -deg180) {
+        deltaLon += deg180;
+        deltaLon += deg180;
+    }
+    distance->x = deltaLon * GPS_cosLat * EARTH_ANGLE_TO_CM; // East-West distance, positive East
+    distance->y = (float)(to->lat - from->lat) * EARTH_ANGLE_TO_CM;  // North-South distance, positive North
+}
+
 void onGpsNewData(void)
 {
     if (!STATE(GPS_FIX)) {
@@ -2600,19 +2622,32 @@ void onGpsNewData(void)
         return;
     }
 
-    gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
+    currentGpsStamp++; // new GPS data available
+
+    gpsDataIntervalSeconds = gpsSol.navIntervalMs * 0.001f; // range for navIntervalMs is constrained to 50 - 2500
+    gpsDataFrequencyHz = 1.0f / gpsDataIntervalSeconds;
 
     GPS_calculateDistanceAndDirectionToHome();
     if (ARMING_FLAG(ARMED)) {
         GPS_calculateDistanceFlown(false);
     }
 
-#ifdef USE_GPS_RESCUE
-    gpsRescueNewGpsData();
-#endif
 #ifdef USE_GPS_LAP_TIMER
     gpsLapTimerNewGpsData();
 #endif // USE_GPS_LAP_TIMER
+
+}
+
+// check if new data has been received since last check
+// if client stamp is initialized to 0, gpsHasNewData will return false until first GPS position update
+// if client stamp is initialized to ~0, gpsHasNewData will return true on first call
+bool gpsHasNewData(uint16_t* stamp) {
+    if (*stamp != currentGpsStamp) {
+        *stamp = currentGpsStamp;
+        return true;
+    } else {
+        return false;
+    }
 }
 
 void gpsSetFixState(bool state)
@@ -2630,6 +2665,11 @@ float getGpsDataIntervalSeconds(void)
     return gpsDataIntervalSeconds;
 }
 
+float getGpsDataFrequencyHz(void)
+{
+    return gpsDataFrequencyHz;
+}
+
 baudRate_e getGpsPortActualBaudRateIndex(void)
 {
     return lookupBaudRateIndex(serialGetBaudRate(gpsPort));

src/main/io/gps.h

@@ -25,15 +25,17 @@
 
 #include "common/axis.h"
 #include "common/time.h"
+#include <common/vector.h>
 
 #include "io/serial.h"
 
 #include "pg/gps.h"
 
 #define GPS_DEGREES_DIVIDER 10000000L
+#define EARTH_ANGLE_TO_CM (111.3195f * 1000 * 100 / GPS_DEGREES_DIVIDER) // 1.113195 cm per latitude unit at the equator (111.3195km/deg)
 #define GPS_X 1
 #define GPS_Y 0
-#define GPS_MIN_SAT_COUNT 4      // number of sats to trigger low sat count sanity check
+#define GPS_MIN_SAT_COUNT 4     // number of sats to trigger low sat count sanity check
 
 #ifdef USE_GPS_UBLOX
 typedef enum {
@@ -222,10 +224,13 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
 } gpsCoordinateDDDMMmmmm_t;
 
 /* LLH Location in NEU axis system */
-typedef struct gpsLocation_s {
-    int32_t lat;                    // latitude * 1e+7
-    int32_t lon;                    // longitude * 1e+7
-    int32_t altCm;                  // altitude in 0.01m
+typedef union gpsLocation_u {
+    struct {
+        int32_t lat;                // latitude * 1e+7
+        int32_t lon;                // longitude * 1e+7
+        int32_t altCm;              // altitude in 0.01m
+    };
+    int32_t coords[3];              // added to provide direct access within loops
 } gpsLocation_t;
 
 /* A value below 100 means great accuracy is possible with GPS satellite constellation */
@@ -246,8 +251,8 @@ typedef struct gpsSolutionData_s {
     gpsLocation_t llh;
     gpsDilution_t dop;
     gpsAccuracy_t acc;
-    uint16_t speed3d;               // speed in 0.1m/s
-    uint16_t groundSpeed;           // speed in 0.1m/s
+    uint16_t speed3d;               // speed in cm/s
+    uint16_t groundSpeed;           // speed in cm/s
     uint16_t groundCourse;          // degrees * 10
     uint8_t numSat;
     uint32_t time;                  // GPS msToW
@@ -282,8 +287,8 @@ typedef struct gpsData_s {
     uint32_t state_position;        // incremental variable for loops
     uint32_t state_ts;              // timestamp for last state_position increment
     uint8_t state;                  // GPS thread state. Used for detecting cable disconnects and configuring attached devices
-    uint8_t userBaudRateIndex;          // index into auto-detecting or current baudrate
-    uint8_t tempBaudRateIndex;          // index into auto-detecting or current baudrate
+    uint8_t userBaudRateIndex;      // index into auto-detecting or current baudrate
+    uint8_t tempBaudRateIndex;      // index into auto-detecting or current baudrate
 
     uint8_t ackWaitingMsgId;        // Message id when waiting for ACK
     ubloxAckState_e ackState;       // Ack State
@@ -301,7 +306,7 @@ typedef struct gpsData_s {
 extern gpsLocation_t GPS_home_llh;
 extern uint16_t GPS_distanceToHome;        // distance to home point in meters
 extern uint32_t GPS_distanceToHomeCm;      // distance to home point in cm
-extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
+extern int16_t GPS_directionToHome;        // direction to home point in degrees * 10
 extern uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
 
 typedef enum {
@@ -389,6 +394,13 @@ void onGpsNewData(void);
 void GPS_reset_home_position(void);
 void GPS_calc_longitude_scaling(int32_t lat);
 void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing);
+
+void GPS_distance2d(const gpsLocation_t *from, const gpsLocation_t *to, vector2_t *distance);
+
 void gpsSetFixState(bool state);
-float getGpsDataIntervalSeconds(void);      // sends GPS Nav Data interval to GPS Rescue
+
+bool gpsHasNewData(uint16_t *stamp);
+float getGpsDataIntervalSeconds(void);  // range 0.05 - 2.5s
+float getGpsDataFrequencyHz(void);      // range 20Hz - 0.4Hz
+
 baudRate_e getGpsPortActualBaudRateIndex(void);

src/main/msp/msp.c

@@ -83,9 +83,7 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
-#include "flight/autopilot.h"
 #include "flight/failsafe.h"
-#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
@@ -119,11 +117,14 @@
 #include "osd/osd_elements.h"
 #include "osd/osd_warnings.h"
 
+#include "pg/autopilot.h"
 #include "pg/beeper.h"
 #include "pg/board.h"
 #include "pg/dyn_notch.h"
+#include "pg/gps_rescue.h"
 #include "pg/gyrodev.h"
 #include "pg/motor.h"
+#include "pg/pos_hold.h"
 #include "pg/rx.h"
 #include "pg/rx_spi.h"
 #ifdef USE_RX_EXPRESSLRS
@@ -1542,9 +1543,9 @@ case MSP_NAME:
         sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM);
         sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
         sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS);
-        sbufWriteU16(dst, autopilotConfig()->throttle_min);
-        sbufWriteU16(dst, autopilotConfig()->throttle_max);
-        sbufWriteU16(dst, autopilotConfig()->hover_throttle);
+        sbufWriteU16(dst, apConfig()->throttle_min);
+        sbufWriteU16(dst, apConfig()->throttle_max);
+        sbufWriteU16(dst, apConfig()->hover_throttle);
         sbufWriteU8(dst,  gpsRescueConfig()->sanityChecks);
         sbufWriteU8(dst,  gpsRescueConfig()->minSats);
 
@@ -1560,9 +1561,9 @@ case MSP_NAME:
         break;
 
     case MSP_GPS_RESCUE_PIDS:
-        sbufWriteU16(dst, autopilotConfig()->altitude_P);
-        sbufWriteU16(dst, autopilotConfig()->altitude_I);
-        sbufWriteU16(dst, autopilotConfig()->altitude_D);
+        sbufWriteU16(dst, apConfig()->altitude_P);
+        sbufWriteU16(dst, apConfig()->altitude_I);
+        sbufWriteU16(dst, apConfig()->altitude_D);
         // altitude_F not implemented yet
         sbufWriteU16(dst, gpsRescueConfig()->velP);
         sbufWriteU16(dst, gpsRescueConfig()->velI);
@@ -1806,7 +1807,11 @@ case MSP_NAME:
     case MSP_RC_DEADBAND:
         sbufWriteU8(dst, rcControlsConfig()->deadband);
         sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
-        sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
+#ifdef USE_POS_HOLD_MODE
+        sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband);
+#else
+        sbufWriteU8(dst, 0);
+#endif
         sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
         break;
 
@@ -2875,9 +2880,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src);
         gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
         gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src);
-        autopilotConfigMutable()->throttle_min = sbufReadU16(src);
-        autopilotConfigMutable()->throttle_max = sbufReadU16(src);
-        autopilotConfigMutable()->hover_throttle = sbufReadU16(src);
+        apConfigMutable()->throttle_min = sbufReadU16(src);
+        apConfigMutable()->throttle_max = sbufReadU16(src);
+        apConfigMutable()->hover_throttle = sbufReadU16(src);
         gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
         gpsRescueConfigMutable()->minSats = sbufReadU8(src);
         if (sbufBytesRemaining(src) >= 6) {
@@ -2898,9 +2903,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         break;
 
     case MSP_SET_GPS_RESCUE_PIDS:
-        autopilotConfigMutable()->altitude_P = sbufReadU16(src);
-        autopilotConfigMutable()->altitude_I = sbufReadU16(src);
-        autopilotConfigMutable()->altitude_D = sbufReadU16(src);
+        apConfigMutable()->altitude_P = sbufReadU16(src);
+        apConfigMutable()->altitude_I = sbufReadU16(src);
+        apConfigMutable()->altitude_D = sbufReadU16(src);
         // altitude_F not included in msp yet
         gpsRescueConfigMutable()->velP = sbufReadU16(src);
         gpsRescueConfigMutable()->velI = sbufReadU16(src);
@@ -2962,7 +2967,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
     case MSP_SET_RC_DEADBAND:
         rcControlsConfigMutable()->deadband = sbufReadU8(src);
         rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
-        rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
+#ifdef USE_POS_HOLD_MODE
+        posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src);
+#else
+        sbufReadU8(src);
+#endif
         flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
         break;
 

src/main/msp/msp_box.c

@@ -57,7 +57,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { .boxId = BOXCAMSTAB, .boxName = "CAMSTAB", .permanentId = 8 },
 //    { .boxId = BOXCAMTRIG, .boxName = "CAMTRIG", .permanentId = 9 },
 //    { .boxId = BOXGPSHOME, .boxName = "GPS HOME", .permanentId = 10 },
-//    { .boxId = BOXGPSHOLD, .boxName = "GPS HOLD", .permanentId = 11 },
+    { .boxId = BOXPOSHOLD, .boxName = "POS HOLD", .permanentId = 11 },
     { .boxId = BOXPASSTHRU, .boxName = "PASSTHRU", .permanentId = 12 },
     { .boxId = BOXBEEPERON, .boxName = "BEEPER", .permanentId = 13 },
 //    { .boxId = BOXLEDMAX, .boxName = "LEDMAX", .permanentId = 14 }, (removed)
@@ -223,6 +223,9 @@ void initActiveBoxIds(void)
         BME(BOXHORIZON);
 #ifdef USE_ALT_HOLD_MODE
         BME(BOXALTHOLD);
+#endif
+#ifdef USE_POS_HOLD_MODE
+        BME(BOXPOSHOLD);
 #endif
         BME(BOXHEADFREE);
         BME(BOXHEADADJ);

src/main/osd/osd.h

@@ -281,6 +281,7 @@ typedef enum {
     OSD_WARNING_OVER_CAP,
     OSD_WARNING_RSNR,
     OSD_WARNING_LOAD,
+    OSD_WARNING_POSHOLD_FAILED,
     OSD_WARNING_COUNT // MUST BE LAST
 } osdWarningsFlags_e;
 

src/main/osd/osd_elements.c

@@ -1048,7 +1048,7 @@ static void osdElementFlymode(osdElementParms_t *element)
     //  1. FS
     //  2. GPS RESCUE
     //  3. PASSTHRU
-    //  4. ANGLE, HORIZON, ACRO TRAINER, ALTHOLD
+    //  4. HEAD, POSHOLD, ALTHOLD, ANGLE, HORIZON, ACRO TRAINER
     //  5. AIR
     //  6. ACRO
 
@@ -1060,10 +1060,12 @@ static void osdElementFlymode(osdElementParms_t *element)
         strcpy(element->buff, "HEAD");
     } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
         strcpy(element->buff, "PASS");
-    } else if (FLIGHT_MODE(ANGLE_MODE)) {
-        strcpy(element->buff, "ANGL");
     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
         strcpy(element->buff, "ALTH");
+    } else if (FLIGHT_MODE(POS_HOLD_MODE)) {
+        strcpy(element->buff, "POSH");
+    } else if (FLIGHT_MODE(ANGLE_MODE)) {
+        strcpy(element->buff, "ANGL");
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         strcpy(element->buff, "HOR ");
     } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {

src/main/osd/osd_warnings.c

@@ -50,6 +50,7 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/pos_hold.h"
 
 #include "io/beeper.h"
 
@@ -64,7 +65,7 @@
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
 
-const char CRASHFLIP_WARNING[] = "> CRASH FLIP <";
+const char CRASHFLIP_WARNING[] = ">CRASH FLIP<";
 
 void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 {
@@ -141,7 +142,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
             *displayAttr = DISPLAYPORT_SEVERITY_INFO;
             return;
         } else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crashflip mode is activated (not allowed / can't happen)
-            tfp_sprintf(warningText, "CRASH FLIP SWITCH");
+            tfp_sprintf(warningText, "CRASHFLIP SW");
             *displayAttr = DISPLAYPORT_SEVERITY_INFO;
             return;
         }
@@ -248,6 +249,15 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 
 #endif // USE_GPS_RESCUE
 
+#ifdef USE_POS_HOLD_MODE
+    if (osdWarnGetState(OSD_WARNING_POSHOLD_FAILED) && posHoldFailure()) {
+        tfp_sprintf(warningText, "POSHOLD FAIL");
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+        *blinking = true;
+        return;
+    }
+#endif
+
     // Show warning if in HEADFREE flight mode
     if (FLIGHT_MODE(HEADFREE_MODE)) {
         tfp_sprintf(warningText, "HEADFREE");
@@ -405,7 +415,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 #ifdef USE_BATTERY_CONTINUE
     // Show warning if battery is not fresh and battery continue is active
     if (hasUsedMAh()) {
-        tfp_sprintf(warningText, "BATTERY CONTINUE");
+        tfp_sprintf(warningText, "BATTERY CONT");
         *displayAttr = DISPLAYPORT_SEVERITY_INFO;
         return;
     }

src/main/pg/alt_hold.c

@@ -1,19 +1,20 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of 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.
+ * Betaflight is 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.
+ *
+ * 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.
  *
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
@@ -29,9 +30,10 @@
 
 #include "alt_hold.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4);
 
-PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig,
-    .alt_hold_target_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
+PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig,
+    .alt_hold_adjust_rate = 50, // max vertical velocity change at full/zero throttle. 50 means 5 m/s
+    .alt_hold_deadband = 20, // throttle deadband in percent of stick travel
 );
 #endif

src/main/pg/alt_hold.h

@@ -1,19 +1,20 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of 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.
+ * Betaflight is 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.
+ *
+ * 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.
  *
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
@@ -24,9 +25,9 @@
 
 #include "pg/pg.h"
 
-typedef struct altholdConfig_s {
-    uint8_t alt_hold_target_adjust_rate;
-} altholdConfig_t;
-
-PG_DECLARE(altholdConfig_t, altholdConfig);
+typedef struct altHoldConfig_s {
+    uint8_t alt_hold_adjust_rate;
+    uint8_t alt_hold_deadband;
+} altHoldConfig_t;
 
+PG_DECLARE(altHoldConfig_t, altHoldConfig);

src/main/pg/autopilot.c

@@ -1,19 +1,20 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of 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.
+ * Betaflight is 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.
+ *
+ * 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.
  *
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
@@ -27,9 +28,9 @@
 
 #include "autopilot.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2);
 
-PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
+PG_RESET_TEMPLATE(apConfig_t, apConfig,
     .landing_altitude_m = 4,
     .hover_throttle = 1275,
     .throttle_min = 1100,
@@ -38,4 +39,10 @@ PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
     .altitude_I = 15,
     .altitude_D = 15,
     .altitude_F = 15,
+    .position_P = 30,
+    .position_I = 30,
+    .position_D = 30,
+    .position_A = 30,
+    .position_cutoff = 80,
+    .max_angle = 50,
 );

src/main/pg/autopilot.h

@@ -1,19 +1,20 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of 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.
+ * Betaflight is 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.
+ *
+ * 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.
  *
- * 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.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
@@ -24,7 +25,7 @@
 
 #include "pg/pg.h"
 
-typedef struct autopilotConfig_s {
+typedef struct apConfig_s {
     uint8_t landing_altitude_m;   // altitude below which landing behaviours can change, metres
     uint16_t hover_throttle;      // value used at the start of a rescue or position hold
     uint16_t throttle_min;
@@ -33,7 +34,13 @@ typedef struct autopilotConfig_s {
     uint8_t altitude_I;
     uint8_t altitude_D;
     uint8_t altitude_F;
-} autopilotConfig_t;
+    uint8_t position_P;
+    uint8_t position_I;
+    uint8_t position_D;
+    uint8_t position_A;
+    uint8_t position_cutoff;
+    uint8_t max_angle;
+} apConfig_t;
 
-PG_DECLARE(autopilotConfig_t, autopilotConfig);
+PG_DECLARE(apConfig_t, apConfig);
 

src/main/pg/pg_ids.h

@@ -84,6 +84,7 @@
 #define PG_GPS_LAP_TIMER 58
 #define PG_ALTHOLD_CONFIG 59
 #define PG_AUTOPILOT 60
+#define PG_POSHOLD_CONFIG 61
 
 // Driver configuration
 #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight

src/main/pg/pos_hold.c

@@ -0,0 +1,39 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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.
+ *
+ * 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_POS_HOLD_MODE
+
+#include "flight/pos_hold.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "pos_hold.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0);
+
+PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
+    .pos_hold_without_mag = false,   // position hold within this percentage stick deflection
+    .pos_hold_deadband = 5,          // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks
+);
+#endif

src/main/pg/pos_hold.h

@@ -0,0 +1,33 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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.
+ *
+ * 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "pg/pg.h"
+
+typedef struct posHoldConfig_s {
+    bool pos_hold_without_mag;
+    uint8_t pos_hold_deadband;
+} posHoldConfig_t;
+
+PG_DECLARE(posHoldConfig_t, posHoldConfig);

src/main/scheduler/scheduler.h

@@ -122,6 +122,9 @@ typedef enum {
 #ifdef USE_ALT_HOLD_MODE
     TASK_ALTHOLD,
 #endif
+#ifdef USE_POS_HOLD_MODE
+    TASK_POSHOLD,
+#endif
 #ifdef USE_MAG
     TASK_COMPASS,
 #endif

src/main/sensors/acceleration.h

@@ -74,7 +74,7 @@ typedef struct rollAndPitchTrims_s {
 } rollAndPitchTrims_t_def;
 
 typedef union rollAndPitchTrims_u {
-    int16_t raw[2];
+    int16_t raw[RP_AXIS_COUNT];
     rollAndPitchTrims_t_def values;
 } rollAndPitchTrims_t;
 

src/main/target/common_post.h

@@ -51,9 +51,12 @@
 #else
 #define FAST_CODE                   __attribute__((section(".tcm_code")))
 #endif
-// Handle case where we'd prefer code to be in ITCM, but it won't fit on the device
 #ifndef FAST_CODE_PREF
 #define FAST_CODE_PREF              FAST_CODE
+// If a particular target is short of ITCM RAM, defining FAST_CODE_PREF in the target.h file will
+// cause functions decorated FAST_CODE_PREF to *not* go into ITCM RAM
+// but if FAST_CODE_PREF is not defined for the target, FAST_CODE_PREF is an alias to FAST_CODE, and
+// functions decorated with FAST_CODE_PREF *will* go into ITCM RAM.
 #endif
 
 #define FAST_CODE_NOINLINE          NOINLINE

src/main/target/common_pre.h

@@ -246,6 +246,7 @@
 #define USE_EMFAT_ICON
 #define USE_ESCSERIAL_SIMONK
 #define USE_ALT_HOLD_MODE
+#define USE_POS_HOLD_MODE
 
 #if !defined(USE_GPS)
 #define USE_GPS

src/main/telemetry/crsf.c

@@ -459,6 +459,8 @@ void crsfFrameFlightMode(sbuf_t *dst)
         flightMode = "ANGL";
     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
         flightMode = "ALTH";
+    } else if (FLIGHT_MODE(POS_HOLD_MODE)) {
+        flightMode = "POSH";
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";
     } else if (isAirmodeEnabled()) {

src/main/telemetry/ibus_shared.c

@@ -250,7 +250,7 @@ static uint16_t getRPM(void)
 static uint16_t getMode(void)
 {
     uint16_t flightMode = 1; //Acro
-    if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE)) {
+    if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE)) {
          flightMode = 0; //Stab
     }
     if (FLIGHT_MODE(PASSTHRU_MODE)) {

src/main/telemetry/ltm.c

@@ -176,7 +176,11 @@ static void ltm_sframe(void)
     else if (FLIGHT_MODE(HORIZON_MODE))
         lt_flightmode = 3;
     else if (FLIGHT_MODE(ALT_HOLD_MODE))
-        lt_flightmode = 5;
+        lt_flightmode = 8;
+    else if (FLIGHT_MODE(POS_HOLD_MODE))
+        lt_flightmode = 9;
+    else if (FLIGHT_MODE(GPS_RESCUE_MODE))
+        lt_flightmode = 13;
     else
         lt_flightmode = 1;      // Rate mode
 

src/main/telemetry/mavlink.c

@@ -475,7 +475,7 @@ void mavlinkSendHUDAndHeartbeat(void)
     // Custom mode for compatibility with APM OSDs
     uint8_t mavCustomMode = 1;  // Acro by default
 
-    if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE | ALT_HOLD_MODE)) {
+    if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE | ALT_HOLD_MODE | POS_HOLD_MODE)) {
         mavCustomMode = 0;      //Stabilize
         mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
     }

src/main/telemetry/smartport.c

@@ -773,7 +773,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                     tmpi += 4;
                 }
 
-                if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE)) {
+                if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE)) {
                     tmpi += 10;
                 }
                 if (FLIGHT_MODE(HORIZON_MODE)) {

src/platform/STM32/target/STM32F745/target.h

@@ -84,5 +84,7 @@
 
 #define FLASH_PAGE_SIZE ((uint32_t)0x8000) // 32K sectors
 
-// ITCM is in short supply so excluding fast code where preferred, not required.
+// ITCM is in short supply for this target.
+// For this target, functions decorated FAST_CODE_PREF will not be put into ITCM RAM; 
+// on other targets, the same function *will* go into ITCM RAM
 #define FAST_CODE_PREF

src/platform/STM32/target/STM32F7X2/target.h

@@ -76,5 +76,7 @@
 
 #define FLASH_PAGE_SIZE ((uint32_t)0x4000) // 16K sectors
 
-// ITCM is in short supply so excluding fast code where preferred, not required.
+// ITCM is in short supply for this target.
+// For this target, functions decorated FAST_CODE_PREF will not be put into ITCM RAM; 
+// on other targets, the same function *will* go into ITCM RAM
 #define FAST_CODE_PREF

src/test/Makefile

@@ -36,8 +36,21 @@ alignsensor_unittest_SRC := \
 		$(USER_DIR)/common/maths.c \
 		$(USER_DIR)/common/vector.c
 
+althold_unittest_SRC := \
+		$(USER_DIR)/flight/alt_hold.c \
+		$(USER_DIR)/flight/autopilot.c \
+		$(USER_DIR)/common/maths.c \
+		$(USER_DIR)/common/vector.c \
+		$(USER_DIR)/common/filter.c \
+		$(USER_DIR)/pg/rx.c
+
+althold_unittest_DEFINES := \
+		USE_ALT_HOLD_MODE= \
+
 arming_prevention_unittest_SRC := \
 		$(USER_DIR)/common/bitarray.c \
+		$(USER_DIR)/common/filter.c \
+		$(USER_DIR)/common/vector.c \
 		$(USER_DIR)/fc/core.c \
 		$(USER_DIR)/fc/dispatch.c \
 		$(USER_DIR)/fc/rc_controls.c \
@@ -46,7 +59,6 @@ arming_prevention_unittest_SRC := \
 		$(USER_DIR)/flight/autopilot.c \
 		$(USER_DIR)/flight/gps_rescue.c
 
-
 arming_prevention_unittest_DEFINES := \
             USE_GPS_RESCUE=
 
@@ -159,6 +171,7 @@ flight_imu_unittest_SRC := \
 		$(USER_DIR)/common/bitarray.c \
 		$(USER_DIR)/common/maths.c \
 		$(USER_DIR)/common/vector.c \
+		$(USER_DIR)/common/filter.c \
 		$(USER_DIR)/config/feature.c \
 		$(USER_DIR)/fc/rc_modes.c \
 		$(USER_DIR)/flight/position.c \
@@ -274,7 +287,6 @@ rx_crsf_unittest_SRC := \
 		$(USER_DIR)/drivers/serial_impl.c
 
 
-
 rx_ibus_unittest_SRC := \
 		$(USER_DIR)/rx/ibus.c
 
@@ -473,15 +485,6 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \
 		USE_GPS= \
 		USE_MSP_OVER_TELEMETRY= \
 
-althold_unittest_SRC := \
-		$(USER_DIR)/flight/alt_hold.c \
-		$(USER_DIR)/flight/autopilot.c \
-		$(USER_DIR)/common/maths.c \
-		$(USER_DIR)/pg/rx.c
-
-althold_unittest_DEFINES := \
-		USE_ALT_HOLD_MODE= \
-
 vtx_msp_unittest_SRC := \
         $(USER_DIR)/common/crc.c \
 		$(USER_DIR)/common/streambuf.c \

src/test/unit/althold_unittest.cc

@@ -15,7 +15,6 @@
  * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
  */
 
-
 #include <stdint.h>
 #include <stdbool.h>
 #include <limits.h>
@@ -26,31 +25,39 @@ extern "C" {
     #include "build/debug.h"
     #include "pg/pg_ids.h"
 
+    #include "common/filter.h"
+    #include "common/vector.h"
+
     #include "fc/core.h"
     #include "fc/rc_controls.h"
     #include "fc/runtime_config.h"
 
     #include "flight/alt_hold.h"
-    #include "flight/autopilot.h"
     #include "flight/failsafe.h"
     #include "flight/imu.h"
     #include "flight/pid.h"
     #include "flight/position.h"
 
+    #include "io/gps.h"
+
     #include "rx/rx.h"
 
+    #include "pg/alt_hold.h"
+    #include "pg/autopilot.h"
+    
     #include "sensors/acceleration.h"
+    #include "sensors/gyro.h"
 
     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
+    PG_REGISTER(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 0);
+    PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0);
+    PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
     PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
-    PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
-    PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
+    PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
 
-    extern altHoldState_t altHoldState;
-    void altHoldInit(void);
-    void updateAltHoldState(timeUs_t);
     bool failsafeIsActive(void) { return false; }
     timeUs_t currentTimeUs = 0;
+    bool isAltHoldActive();
 }
 
 #include "unittest_macros.h"
@@ -63,56 +70,69 @@ uint32_t millis() {
 
 TEST(AltholdUnittest, altHoldTransitionsTest)
 {
-    updateAltHoldState(currentTimeUs);
-    EXPECT_EQ(altHoldState.isAltHoldActive, false);
+    updateAltHold(currentTimeUs);
+    EXPECT_EQ(isAltHoldActive(), false);
 
     flightModeFlags |= ALT_HOLD_MODE;
     millisRW = 42;
-    updateAltHoldState(currentTimeUs);
-    EXPECT_EQ(altHoldState.isAltHoldActive, true);
+    updateAltHold(currentTimeUs);
+    EXPECT_EQ(isAltHoldActive(), true);
 
     flightModeFlags ^= ALT_HOLD_MODE;
     millisRW = 56;
-    updateAltHoldState(currentTimeUs);
-    EXPECT_EQ(altHoldState.isAltHoldActive, false);
+    updateAltHold(currentTimeUs);
+    EXPECT_EQ(isAltHoldActive(), false);
 
     flightModeFlags |= ALT_HOLD_MODE;
     millisRW = 64;
-    updateAltHoldState(currentTimeUs);
-    EXPECT_EQ(altHoldState.isAltHoldActive, true);
+    updateAltHold(currentTimeUs);
+    EXPECT_EQ(isAltHoldActive(), true);
 }
 
 TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter)
 {
     altHoldInit();
-    EXPECT_EQ(altHoldState.isAltHoldActive, false);
+    EXPECT_EQ(isAltHoldActive(), false);
 
     flightModeFlags |= ALT_HOLD_MODE;
     millisRW = 42;
-    updateAltHoldState(currentTimeUs);
-    EXPECT_EQ(altHoldState.isAltHoldActive, true);
+    updateAltHold(currentTimeUs);
+    EXPECT_EQ(isAltHoldActive(), true);
 }
 
 // STUBS
 
 extern "C" {
-    acc_t acc;
+    uint8_t armingFlags = 0;
+    int16_t debug[DEBUG16_VALUE_COUNT];
+    uint8_t debugMode;
+    uint16_t flightModeFlags = 0;
+    uint8_t stateFlags = 0;
 
-    float getAltitudeCm(void) {return 0.0f;}
-    float getAltitudeDerivative(void) {return 0.0f;}
+    acc_t acc;
+    attitudeEulerAngles_t attitude;
+    gpsSolutionData_t gpsSol;
+
+    float getAltitudeCm(void) { return 0.0f; }
+    float getAltitudeDerivative(void) { return 0.0f; }
     float getCosTiltAngle(void) { return 0.0f; }
+    float getGpsDataIntervalSeconds(void) { return 0.01f; }
+    float getGpsDataFrequencyHz(void) { return 10.0f; }
     float rcCommand[4];
 
-    void parseRcChannels(const char *input, rxConfig_t *rxConfig)
-    {
+    bool gpsHasNewData(uint16_t* gpsStamp) {
+        UNUSED(*gpsStamp);
+        return true;
+    }
+
+    void GPS_distance2d(const gpsLocation_t* /*from*/, const gpsLocation_t* /*to*/, vector2_t* /*dest*/) { }
+
+    void parseRcChannels(const char *input, rxConfig_t *rxConfig) {
         UNUSED(input);
         UNUSED(rxConfig);
     }
 
-    int16_t debug[DEBUG16_VALUE_COUNT];
-    uint8_t debugMode;
-
-    uint8_t armingFlags = 0;
-    uint8_t stateFlags = 0;
-    uint16_t flightModeFlags = 0;
+    throttleStatus_e calculateThrottleStatus() {
+        return THROTTLE_LOW;
+    }
 }

src/test/unit/arming_prevention_unittest.cc

@@ -36,7 +36,6 @@ extern "C" {
     #include "fc/rc_modes.h"
     #include "fc/runtime_config.h"
 
-    #include "flight/autopilot.h"
     #include "flight/failsafe.h"
     #include "flight/gps_rescue.h"
     #include "flight/imu.h"
@@ -48,11 +47,13 @@ extern "C" {
     #include "io/beeper.h"
     #include "io/gps.h"
 
+    #include "pg/autopilot.h"
     #include "pg/gps_rescue.h"
     #include "pg/motor.h"
+    #include "pg/rx.h"
+
     #include "pg/pg.h"
     #include "pg/pg_ids.h"
-    #include "pg/rx.h"
 
     #include "rx/rx.h"
 
@@ -78,7 +79,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(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
+    PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0);
 
     float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
     uint16_t averageSystemLoadPercent = 0;
@@ -100,9 +101,7 @@ extern "C" {
     uint8_t activePidLoopDenom = 1;
 
     float getGpsDataIntervalSeconds(void) { return 0.1f; }
-    void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) { filter->k = k; }
-    void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
-    void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) { filter->k = k; }
+    float getGpsDataFrequencyHz(void) { return 10.0f; }
 }
 
 uint32_t simulationFeatureFlags = 0;
@@ -1140,24 +1139,30 @@ extern "C" {
     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 pt1FilterInit(pt1Filter_t *velocityDLpf, float) {
-        UNUSED(velocityDLpf);
-    }
-    float pt1FilterApply(pt1Filter_t *velocityDLpf, float) {
-        UNUSED(velocityDLpf);
-        return 0.0f;
-    }
-    void pt3FilterInit(pt3Filter_t *velocityUpsampleLpf, float) {
-        UNUSED(velocityUpsampleLpf);
-    }
-    float pt3FilterApply(pt3Filter_t *velocityUpsampleLpf, float) {
-        UNUSED(velocityUpsampleLpf);
-        return 0.0f;
-    }
+    float sin_approx(float) {return 0.0f;}
+    float cos_approx(float) {return 1.0f;}
+    float atan2_approx(float, float) {return 0.0f;}
+
     void getRcDeflectionAbs(void) {}
     uint32_t getCpuPercentageLate(void) { return 0; }
     bool crashFlipSuccessful(void) { return false; }
+
+    void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing)
+    {
+       UNUSED(from);
+       UNUSED(to);
+       UNUSED(dist3d);
+       UNUSED(dist);
+       UNUSED(bearing);
+    }
+
+void GPS_distance2d(const gpsLocation_t* /*from*/, const gpsLocation_t* /*to*/, vector2_t* /*dest*/) { }
+
+    bool canUseGPSHeading;
+    bool compassIsHealthy;
+
+    bool gpsHasNewData(uint16_t* gpsStamp) {
+         UNUSED(*gpsStamp);
+         return true;
+     }
 }

src/test/unit/flight_imu_unittest.cc

@@ -42,7 +42,6 @@ extern "C" {
     #include "fc/runtime_config.h"
     #include "fc/rc.h"
 
-    #include "flight/autopilot.h"
     #include "flight/imu.h"
     #include "flight/mixer.h"
     #include "flight/pid.h"
@@ -52,6 +51,8 @@ extern "C" {
 
     #include "rx/rx.h"
 
+    #include "pg/autopilot.h"
+
     #include "sensors/acceleration.h"
     #include "sensors/barometer.h"
     #include "sensors/compass.h"
@@ -74,7 +75,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(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
+    PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0);
 
     PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
         .enabledFeatures = 0
@@ -435,15 +436,7 @@ extern "C" {
     bool schedulerGetIgnoreTaskExecTime() { return false; }
     float gyroGetFilteredDownsampled(int) { return 0.0f; }
     float baroUpsampleAltitude()  { return 0.0f; }
-    float pt2FilterGain(float, float)  { return 0.0f; }
     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);
-    }
-    float pt2FilterApply(pt2Filter_t *baroDerivativeLpf, float) {
-        UNUSED(baroDerivativeLpf);
-        return 0.0f;
-    }
 }

src/test/unit/platform.h

@@ -37,9 +37,11 @@
 #define NOINLINE
 #define FAST_CODE
 #define FAST_CODE_NOINLINE
+#define FAST_CODE_PREF
 #define FAST_DATA_ZERO_INIT
 #define FAST_DATA
 
+
 #define PID_PROFILE_COUNT 4
 #define CONTROL_RATE_PROFILE_COUNT  4
 #define USE_MAG

src/test/unit/vtx_unittest.cc

@@ -212,5 +212,5 @@ extern "C" {
     void sbufWriteU32(sbuf_t *, uint32_t) {}
     void schedulerSetNextStateTime(timeDelta_t) {}
     bool crashFlipSuccessful(void) {return false; }
-
+    bool canUseGPSHeading;
 }