Converge IMU faster at higher groundspeed during GPS Rescue #12738

ctzsnooze · 2023-04-28T15:30:31Z

Tested, works well, but would appreciate further testing please!

Makes the following changes:

Yaw IMU gain increases up to 6x when there is a large drift over ground error, so that the IMU can re-orient itself more quickly the worse the error
locks yaw IMU gain to zero during climb and rotate phases, preventing acquisition of IMU orientation errors due to drift during long climbs
attenuates the maximum angle allowed when there is a large drift over ground error, to reduce the speed of the flyaway that can happen when the IMU orientation is very incorrect
max rescue angle defaults to 45 degrees. Even with a maximum IMU error, 45 degrees max angle doesn't cause the quad to speed off madly into the distance.
fixes an issue where direct entry to a descent mode may not gain full yaw authority
changes the sensitivity of the FLY HOME sanity check. Now the sanity check will only count up towards to the 15s disarm value if the quad fails to reach 10%, not 50%, of the set return speed, when checked each second. As before this is an up-down counter, so any time the quad attains >10% of the set return velocity to home, the counter decrements by 1, towards a minimum of zero, and when less than 10%, the counter increments by one. If there is a net cumulative period of more than 15s with less than 10% forward speed to home, the sanity check will disarm the quad, since it is most unlikely that it will return home. Users should expect some counting up in the early part of a rescue, especially with bad IMU disorientation, but with the above changes the quad should not disarm from this sanity check unless something really has gone badly wrong, eg stuck in a tree, or crashed, or extremely high wind.

WARNING 1:
A GPS-only rescue system cannot handle high wind drift - do NOT rely on GPS Rescue in winds above 50-60kph unless you have a properly calibrated and fully functional, tested, Magnetometer (compass). Higher maximum angle will help handle strong winds better, but the max throttle must be increased also, and in the face of a temporary IMU error, the quad may fly off in the wrong direction at high speed. In practice, a 35-45 degree angle is more than enough for normal conditions, a 50-55 degree angle may be better in stronger winds. Always test the return in the conditions of the day to ensure your quad will turn and head for home effectively. If the wind is too high, the quad will kind of spiral around, at speed, making little or no headway. Don't let it do this for more than 15s or it will sanity check and drop.

WARNING 2:
IMU disorientation is always possible with GPS-only systems. If a rescue is initiated and the IMU is disoriented from prior drift or high wind, the quad may initially head off in completely the wrong direction, even though the arrow points straight up. Functionally the quad thinks it is flying home, but has temporarily got the orientation of the quad wrong. The IMU will, or should, correct itself over the next 2-3seconds, but will fly in a large arc while doing so. If the maxim allows angle in the rescue is set high, this arc may be flown at high speed, and cover some considerable distance, but the quad will likely handle higher wind rescues a bit more reliably.

To test: enter a GPS Rescue after a 6-8s period of tail drift (fly backwards at > 2m/s for say 6-10s). Hold that drift long enough for the arrow to rotate 180 degrees wrong. Initiate the rescue. The rescue will then rotate entirely the wrong direction, ie fly directly away from home. With this PR it should gain speed away from home for less than a second, and then smoothly and steadily, over 2-3 seconds rotate towards home. The arrow will keep pointing straight up all the time (the turning is the IMU correcting itself). After maybe 2-4s, it should be heading home. The quad will attain a decent speed while correcting itself. The max speed is determined mostly by the maximum angle.

If the debug mode RTH is displayed on the OSD screen, the sanity check timer is the value of the last two digits of the last debug element.

Testing in high wind and/or with prior extreme back drift would be appreciated.
Testing to figure out how much wind can be tolerated would be helpful, too.

Thanks to @sek101 for investigating and clarifying the problem. To summarise, the problem is that:

if the quad drifts in a direction other than in the direction of the nose of the quad, and
the drift is at a significant speed (anything > 2m/s), and
the drift is sustained for at least one or two dcm_kp time constants (6-8 seconds)
the IMU will, over time, consider that the nose of the quad is pointing in the direction of the course over ground, not the true heading of the quad, and point the arrow to the bottom of the goggles, thinking that is where home is.
Then, when a rescue is initiated, the quad will rotate so that the arrow points upwards, and will fly in a direction that can be entirely incorrect, including a full 180 degrees wrong. It will also fly faster than usual at first, and may make a large arc in the sky before finally settling on the true path to home.

This excellent video from Sek101 shows a takeoff, then a quick rotation to fly away from home backwards. The home arrow slowly orients itself to become 180 degrees wrong. When the rescue is initiated, the quad rotates according to the incorrect IMU orientation, and flies off 180 degrees in the wrong direction.
https://www.youtube.com/watch?v=DyEYdI-Qgtw

Code attempt 1 = FAILURE!
This idea was simple: make the dcm_kp value proportional to groundspeed during a GPS Rescue, using about a 4x increase in dcm_kp at normal rescue speed, and very low dcm_kp at low/zero groundspeed.
RESULT: spirals during initiation when IMU error existed at the start, quad flies off very fast, then enters a kind of "chasing the tail" behaviour.
Video shows, around 44s, quad in reverse, arrow going wrong, then persisting rotation.
https://www.youtube.com/watch?v=BSOgY-f1XtM

Code attempt 2 = NOT QUITE THERE YET!
Here we detect an absolute error between groundspeed and velocity to home, normalised to the intended return speed, plus one. If the quad is flying directly home, there is no velocity error, so this groundspeedErrorRatio value would be 0+1 , ie 1.0. If the quad is flying off at 90 degrees to home at a groundspeed equal to the intended return speed, the groundspeedErrorRatio would be 2.0. If flying off with a full 180 degree error, at the intended return speed, this factor would be 3.0.
Then we use this factor to boost dcm_kp during the rescue.
RESULT: Some improvement, but still tended to spiral and pick up a lot of speed.

Code attempt 3 = very good!

calculate groundspeedErrorRatio as above, and use it to boost dcm_kp up to 5 times.
use the inverse of this value to attenuate the maximum allowed pitch angle, thereby limiting absolute velocity when the IMU error would send the quad in the wrong direction.
set dcm_kp forcibly to zero while in the climb or rotate phases of the rescue. This locks in the pre-existing IMU state, which is normally pitch forward flight with a high likelihood of pointing to home. By locking in that IMU state, we prevent drift (from wind, etc) from building up an IMU error of the kind that could cause a flyaway.
finally, restore the max angle to the previous default of 40, for pilots who may have GPS modules that aren't working well, to avoid excessive pitch error issues in those cases.

Here is some low-res video of the IMU problem and how this PR achieves a pretty good result in the face of the worst possible IMU errors.

https://www.youtube.com/watch?v=IKbeWCsQj24

This PR should significantly reduce the chance that a working GPS Rescue setup would fail, especially if there is, or has been, signficant sideways or rear-wards drift either for a period of time before the rescue was initiated, or during the climb period. Previous versions of GPS Rescue were very susceptible to IMU disorientation and could enter persistent non-recovering flyaways.

ledvinap · 2023-04-28T15:56:06Z

(Did I mention Kalman filter in IMU this year?)

blckmn · 2023-04-28T17:17:20Z

AUTOMERGE: (FAIL)

github identifies PR as mergeable -> PASS
assigned to a milestone -> PASS
cooling off period lapsed -> PASS
commit count less or equal to three -> FAIL
Don't merge label NOT found -> PASS
at least one RN: label found -> PASS
Tested label found -> FAIL
assigned to an approver -> PASS
approver count at least three -> FAIL

src/main/flight/gps_rescue.c

src/main/common/maths.h

ledvinap

OK if it works in practice.
But remove M_HALF_PIf, M_TWO_PIf

src/main/flight/gps_rescue.c

src/main/flight/imu.c

ctzsnooze · 2023-05-09T23:51:33Z

@ledvinap @KarateBrot... I accepted your comments above and:

removed the refactoring involving PI,
ensured that the IMU gain is only modified when we are in the GPS Rescue flight mode
modified a comment

Both Sek101 and I have tested this code now, and it works very well. The higher gain for the IMU boost factor helps deal with IMU errors more quickly when needed, and does not induce circling or excessive overshoot, even with 120kph backwards flight with a full 180 degree IMU error. I think the gain factor is about as high as it should be.

I'm very grateful for your advice and assistance with this PR.

A final check, and if agreeable, approval, would be much appreciated. I am confident that it will make GPS rescues much safer.

ledvinap · 2023-05-10T08:50:48Z

src/main/flight/gps_rescue.c

@@ -567,6 +569,25 @@ static void sensorUpdate(void)
    rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
    rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s

+    // when there is significant velocity error, increase IMU COG Gain for yaw to a higher value, and reduce max pitch angle
+    if (gpsRescueConfig()->rescueGroundspeed) {
+        const float rescueGroundspeed = 1000.0f; // equivalent of fixed 10 m/s groundspeed


Suggested change

const float rescueGroundspeed = 1000.0f; // equivalent of fixed 10 m/s groundspeed

const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed

OK, I'll make that change.

ledvinap · 2023-05-10T08:54:16Z

src/main/flight/gps_rescue.c

@@ -648,10 +669,13 @@ void descend(void)
        rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * rescueState.intent.proximityToLandingArea;
        // reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
        // if quad drifts further than 2m away from home, should by then have rotated towards home, so pitch is allowed
-        rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
+        rescueState.intent.rollAngleLimitDeg = 0.5f * gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;


This changes scaling of user parameter. gpsRescueConfig()->maxRescueAngle shall be commented in CLI (only half is used for roll limit)

I can document this behaviour in the wiki, but I'm not sure how to make a 'comment in the CLI' for the user to read.
Roll is significantly weaker than pitch in the rescue, and just about never reaches the limit of half the pitch angle. The main consideration is the limit on pitch, that's the one that concerns people the most.
The intent here is that if the user changes the pitch angle to a very low value, that the roll angle stays in approximate proportion to the pitch angle change.
The actual roll angle depends on yaw rate, and also on the amount of roll added by earth referencing code in Angle mode. By limiting the GPS part of the roll angle to half that of pitch, the user will likely not exceed the set max angle even with earth referenced yaw enabled.

I think code comment is enough. As you say, the 50% limit is more than enough. But make it easier for programmers to understand it and avoid confusion.

Will do this in separate PR

src/main/flight/imu.c

ledvinap · 2023-05-10T09:00:43Z

src/main/flight/imu.c

-
+        if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+            cogYawGain = gpsRescueGetImuYawCogGain(); // do not modify IMU yaw gain unless in a rescue
+        }


Suggested change

}

} else {

cogYawGain = 1.0f;

}

CoG is used to correct yaw in forward flight. Ideally, thrust vector horizontal vector shall be computed from roll/pitch and used to adjust courseOverGround. But for now, don't change existing behavior.

Here I think the alternative,or else condition, on line 498, should be a zero value, not a 1, to return the float equivalent of the boolean OFF. That's why I initialise cogYawGain to 0.0f on line 477. Our intent is to do nothing unless we are in FLIGHT_MODE(GPS_RESCUE_MODE). If we keep cogYawGain at zero, it won't pass the check for if (cogYawGain != 0.0f) back on line 213.

I agree it would be great to get a vector from roll/pitch attitude to help evaluate courseOverGround, especially if integrated over time, and also agree that for now I'd not want to change the existing behaviour.

Original code did set useCOG = true;
https://github.com/betaflight/betaflight/pull/12738/files#diff-de6b2b61376cbb30512686ebb362260facb69cf9a4281ebdcd59af61c0306f7aL498

This change would disable GPS yaw correction unless in GPS_RESCUE_MODE. It may be desirable, but it is unrelated to this PR.

@ledvinap Looks like this was merged before I had time to resolve this.

I'll make a small PR to restore the original behaviour. I'm not 100% exactly sure what is best, but I guess we should correct the quad's attitude throughout the flight, since otherwise it could drift. And in any case I had no intention of altering the existing behaviour.

Also I was reflecting on your comment, earlier, about using Pitch angle as a factor for IMU gain.

This could be done quite easily in the GPS Rescue case, and would be beneficial in the situation of a rescue initiated with very low groundspeed but an IMU that is 180 degrees wrong. Currently the quad will pitch forward, but it takes time to accumulate velocity before the IMU gain for yaw is increased. If we had a signal to boost IMU directly related to forward pitch angle (before adjustment for groundspeed error), this is nearly always high to maximal in groundspeed error settings, and is high from the moment the rescue starts. Using positive pitch angle to generate IMU yaw gain would act as a leading indicator of a 180 degree IMU error. Also, generally speaking, it is more likely that if the pitch angle is positive, then we should have direction over ground that corresponds 'better than usually' to the orientation of the nose of the quad.

A flight home at high angle against a strong headwind, or with a high target velocity, could require a high pitch angle all the time. The quad would then fly home with an increased IMU gain on yaw the whole time. In both these situations we should be pointing fairly directly to home, and the IMU error in any case resolves to reduce the effect of sideways drag. IMU yaw gain is functionally a time constant, so we are not going to change the magnitude of anything, just how quickly it happens, and this is probably not so bad in higher pitch angle environments.

I'm inclined to test this out. It will require some heuristic gain factor though :-)

Thanks again for all your input to date, I very much appreciate everything you've done here.

github-actions · 2023-05-10T13:24:33Z

Do you want to test this code? Here you have an automated build:
Assets
WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

sugaarK · 2023-05-10T20:00:50Z

I flew a number of packs with this yesterday and it works VERY well one warning is not to stand to close to your launch point as it’ll try and land on your head 😂

sugaarK · 2023-05-10T20:01:11Z

I flew a number of packs with this yesterday and it works VERY well one warning is not to stand to close to your launch point as it’ll try and land on your head 😂

sugaarK

I flew a number of packs with this yesterday and it works VERY well one warning is not to stand to close to your launch point as it’ll try and land on your head 😂

@KarateBrot

…ht#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

ctzsnooze · 2023-05-11T01:16:21Z

@ledvinap I've tried to address your remaining comments in #12789

@KarateBrot

…12785) 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 Co-authored-by: ctzsnooze <chris.thompson@sydney.edu.au>

@KarateBrot

…ht#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

@KarateBrot

…ht#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

ctzsnooze self-assigned this Apr 28, 2023

ctzsnooze added the Don't Merge label Apr 28, 2023

This comment has been minimized.

Sign in to view

ctzsnooze force-pushed the imuCalcKpGain-changes branch from 1433ddc to 4f3d45e Compare April 28, 2023 15:56

This comment has been minimized.

Sign in to view

ctzsnooze force-pushed the imuCalcKpGain-changes branch from 4f3d45e to 273c65d Compare April 29, 2023 01:57

This comment has been minimized.

Sign in to view

ctzsnooze force-pushed the imuCalcKpGain-changes branch 2 times, most recently from a3a13ca to b9121e9 Compare April 29, 2023 03:16

This comment has been minimized.

Sign in to view

KarateBrot reviewed Apr 29, 2023

View reviewed changes

src/main/flight/gps_rescue.c Outdated Show resolved Hide resolved

This comment has been minimized.

Sign in to view

ctzsnooze force-pushed the imuCalcKpGain-changes branch from 94c0009 to 5f864f9 Compare April 30, 2023 00:31

This comment has been minimized.

Sign in to view

ctzsnooze force-pushed the imuCalcKpGain-changes branch 4 times, most recently from b5374d8 to 03ce10e Compare April 30, 2023 11:22

This comment has been minimized.

Sign in to view

ctzsnooze added Testing Required Ready to Merge and removed Don't Merge labels Apr 30, 2023

ctzsnooze requested review from blckmn and haslinghuis April 30, 2023 11:46

KarateBrot requested changes May 9, 2023

View reviewed changes

src/main/common/maths.h Outdated Show resolved Hide resolved

ledvinap approved these changes May 9, 2023

View reviewed changes

src/main/flight/gps_rescue.c Show resolved Hide resolved

src/main/flight/imu.c Outdated Show resolved Hide resolved

Fix unit test

f4aad6c

ctzsnooze force-pushed the imuCalcKpGain-changes branch from 3908908 to e00970b Compare May 9, 2023 23:05

This comment has been minimized.

Sign in to view

ensure cogYawGain is zero, except during gps rescue

7b8ed25

ctzsnooze force-pushed the imuCalcKpGain-changes branch from e00970b to 7b8ed25 Compare May 9, 2023 23:24

This comment has been minimized.

Sign in to view

update comment after suggestion from ledvinap

a0e9e24

This comment has been minimized.

Sign in to view

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

fbd8c48

This comment has been minimized.

Sign in to view

ctzsnooze mentioned this pull request May 10, 2023

Improve behaviour when GPS Rescue is initiated close to home #12715

Merged

ledvinap reviewed May 10, 2023

View reviewed changes

changes to comments, thanks petr

1649b2e

KarateBrot approved these changes May 10, 2023

View reviewed changes

sugaarK approved these changes May 10, 2023

View reviewed changes

haslinghuis merged commit 8e3389f into betaflight:master May 10, 2023
13 of 19 checks passed

haslinghuis mentioned this pull request May 10, 2023

[4.4.2] Converge IMU faster at higher groundspeed during GPS Rescue #12785

Merged

ctzsnooze mentioned this pull request May 11, 2023

GPS Rescue Bugfix to ensure IMU adaptation to GPS course over ground #12789

Merged

ctzsnooze deleted the imuCalcKpGain-changes branch April 3, 2024 06:08

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Converge IMU faster at higher groundspeed during GPS Rescue #12738

Converge IMU faster at higher groundspeed during GPS Rescue #12738

ctzsnooze commented Apr 28, 2023 •

edited

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ledvinap commented Apr 28, 2023

This comment has been minimized.

blckmn commented Apr 28, 2023 •

edited

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ledvinap left a comment

This comment has been minimized.

This comment has been minimized.

ctzsnooze commented May 9, 2023

This comment has been minimized.

This comment has been minimized.

ledvinap May 10, 2023

ctzsnooze May 10, 2023

ledvinap May 10, 2023

ctzsnooze May 10, 2023

ledvinap May 10, 2023

ctzsnooze May 11, 2023

ledvinap May 10, 2023

ctzsnooze May 10, 2023

ledvinap May 10, 2023

ctzsnooze May 10, 2023

github-actions bot commented May 10, 2023

sugaarK commented May 10, 2023

sugaarK commented May 10, 2023

sugaarK left a comment

ctzsnooze commented May 11, 2023

	const float rescueGroundspeed = 1000.0f; // equivalent of fixed 10 m/s groundspeed
	const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed

-        }
+        } else {
+            cogYawGain = 1.0f;
+        }

Converge IMU faster at higher groundspeed during GPS Rescue #12738

Converge IMU faster at higher groundspeed during GPS Rescue #12738

Conversation

ctzsnooze commented Apr 28, 2023 • edited

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ledvinap commented Apr 28, 2023

This comment has been minimized.

blckmn commented Apr 28, 2023 • edited

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ledvinap left a comment

Choose a reason for hiding this comment

This comment has been minimized.

This comment has been minimized.

ctzsnooze commented May 9, 2023

This comment has been minimized.

This comment has been minimized.

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

github-actions bot commented May 10, 2023

sugaarK commented May 10, 2023

sugaarK commented May 10, 2023

sugaarK left a comment

Choose a reason for hiding this comment

ctzsnooze commented May 11, 2023

ctzsnooze commented Apr 28, 2023 •

edited

blckmn commented Apr 28, 2023 •

edited