Converge IMU faster at higher groundspeed during GPS Rescue (#12738)
commit8e3389f452658bfdc479cba9139a45b2e2b79cad
authorctzsnooze <chris.thompson@sydney.edu.au>
Wed, 10 May 2023 20:24:34 +0000 (11 06:24 +1000)
committerGitHub <noreply@github.com>
Wed, 10 May 2023 20:24:34 +0000 (10 22:24 +0200)
tree5e58469041c393f2d89b24745a585ffcf84c013d
parent9ff4723c1918613d0e84b4c1fb22deb1c1fd163a
Converge IMU faster at higher groundspeed during GPS Rescue (#12738)

* dynamic dcm_kp depending on groundspeed vs velocity to home

* reduce max pitch angle default to 40

* Only 10% of set velocity to home is enough, angle to 45

to reduce chance of sanity check against headwind

* fixes, thanks @KarateBrot

* Feedback from Ledvinap, ensure full pitch and yaw authority in descend

* pitch limit max of 1/3 and dcm_kp gain to 6x max

* apply IMU kP factor to yaw only via ez_ef

* More from @Ledvinap, constrain pitch in descent also

* Fix the velocity error gain, refactoring thanks to Ledvinap

* thanks, Mark!

* fixes, must be fabsf

* make IMU gain twice as sensitive to groundspeed error

* useCOG as float

* Fix unit test

* ensure cogYawGain is zero, except during gps rescue

* update comment after suggestion from ledvinap

* keep roll angle limit consistently at 50% of pitch limit when active

* changes to comments, thanks petr
src/main/flight/gps_rescue.c
src/main/flight/gps_rescue.h
src/main/flight/imu.c
src/main/pg/gps_rescue.c
src/test/unit/flight_imu_unittest.cc