Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

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

Browse source 
* 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>
This commit is contained in:
ctzsnooze 2024-12-06 13:04:00 +11:00 • committed by GitHub
parent 23605feb79
commit 3138141cd1
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
66 changed files with 1234 additions and 472 deletions
Download patch file Download diff file Expand all files Collapse all files

41
src/main/msp/msp.c
View file

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