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 \