GPS rescue IMU, velocity iTerm and other fixes

[ctzsnooze]

This PR is based on the well-tested PR #12853, but is intended for merging.

It contains updates and improvements made possible by @ledvinap 's PR #12792, which provides key fixes that prevents IMU orientation errors from developing when the quad is flown in directions other than nose-forward, and ensures that the IMU will update and correct even if a 180 degree IMU error exists.

I intend to keep 12853 un-merged for ongoing GPS testing purposes. Currently it has the same code as this PR, but with one change that allows IMU disorientation to develop, purely for testing purposes.

Summary of changes:

1. More rapidly adapts the quad's IMU yaw orientation to the GPS course over ground when:

• the pitch angle of the quad is higher and
• when the quad is flying away from home, rather than towards home (ie, when we must have an IMU disorientation problem).
  Together, these two factors boost the IMU yaw Cog gain up to 5 times normal when the IMU is disoriented at the start of a rescue, helping it correct faster and minimising the flyaway distance.

2. When there is no IMU disorientation, ie when the quad is flying towards home during the rescue, the IMU yaw gain value is set in proportion to the requested pitch angle. IMU gain will be zero when flat, normal at 30 degrees forward pitch angle and double normal at 60 degrees forward pitch angle. Effectively this prevents the development of an IMU disorientation problem while climbing, rotating or descending (very low pitch angles), even if the quad was to drift rapidly, but ensures the IMU is updated actively if flying home against wind.

3. Provides a CLI value GPS_RESCUE_IMU_YAW_GAIN that adjusts the initial correction radius when there is an IMU error. Default is 10, with the range 5-20. Larger numbers result in a wider radius, and vice versa. This factor adjusts the strength of the IMU gain factor when correcting a flyaway due to a pre-existing IMU error. In a full 180 degree IMU error, the quad will initially fly diametrically away from home, but still must fly pitched forward for a time, while the IMU corrects. As the IMU error corrects, the quad yaws towards home while pitched forward. A tighter turn radius (smaller value) minimises the distance traveled while correcting the IMU error. Lower values be OK in light winds or where available turn space is limited, but can also result in yaw overshoot and oscillation when initially correcting the IMU, and may result in a spiraling behaviour, causing a rescue failure. A wider radius (higher number) leads to a greater distance traveled in the early part of the flyaway correction, but gives more time in the upwind phase, and is less likely to spiral. The default value works well in most cases.

4. Modifies the normal IMU yaw heading adaptation to the GPS course over ground, so that instead of being 'absolutely no adaptation below 2m/s groundspeed, and normal adaptation immediately we exceed 1m/s', the code now:

• has zero adaptation below 1m/s groundspeed
• linearly increases IMU gain from 20% of normal at 1m/s to normal at 5m/s
• continues to increase IMU gain to 200% of normal at 10m/s
  This combines with the IMU changes in #12792to improve the ability of the IMU to adapt to the course over ground under conditions that most likely reflect the 'real' flight path of the machine, rather than a part associated with drift. In essence, this favours updating the IMU more rapidly when both the quad is significantly angled away from 'flat' (actively generating thrust) but also when it has significant velocity over ground. It also allows slow adaptation even if the pilot flies relatively slowly after takeoff.

5. Angle mode earth referencing is forced ON in a GPS rescue, since otherwise if the user was to disable earth referencing in angle mode, the lack of roll during yaws while pitching forward significantly weakens the precision of the rescue, especially in high wind.

6. Implements a bugfix for an issue that was preventing velocity iTerm being applied in the fly home phase

7. Includes modifications to velocity iTerm to prevent unwanted accumulation

8. Increases the allowed sanity check failure time to 30s, to give users plenty of time to realise they should give up and take over manual control. This extra time should help users give their quad enough time to determine if a rescue will be successful.

9. Modifies the ATTITUDE debug to track a number of internal IMU variables; note that GPS groundspeed and course over ground are available in every log.

0 = accADC[X] // X axis attitude, as per original debug
1 = accADC[Y] // Y axis attitude, as per original debug
2 = IMU CogYaw gain value * 100 // dynamically altered by groundspeedErrorRatio and pitchForwardAngle in a rescue
3 = ez_ef value * 100 (after multiplication by IMU CogYaw gain when in a rescue)
4 = velocity to home in cm/s
5 = groundspeedErrorRatio * 100 (200 means the error between groundspeed and speed to home is 20ms)
6 = pitchForwardAngle * 100 (value of 100 means 30 degree pitch forward, 200 means 60 degrees)
7 = dcmKpGain * 100 // for testing dcmKpGain refactor or dcmKpGain code changes

Notes:

• Please test carefully when the quad must return against head-winds greater than 40kph. It's important to ensure the quad can make it home before relying on the rescue.
• I would be grateful if people could help identify the windspeed at which a GPS rescue fails
• Under-powered or high-drag quads may need higher angle and higher maximum GPS throttle
• Very powerful quads may need a very low minimum GPS throttle to descend successfully on windy days

[github-actions]

Do you want to test this code? You can flash it directly from Betaflight Configurator:

• Simply put #12900 (this pull request number) in the Select commit field of the Configurator firmware flasher tab (you need to Enable expert mode, Show release candidates and Development).

WARNING: It may be unstable. Use only for testing!

[blckmn]

AUTOMERGE: (FAIL)

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

[ctzsnooze]

I show how this PR handles 180 degrees IMU errors in this test, done with PR 12853, which has the code of 12900 minus the ability to prevent IMU error generation by flying backwards. Here is a video of the test flight:

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

With 12900 it is impossible to generate these kinds of IMU errors, or at least I cannot create them. The rescues in the above video, which all had full 180 IMU errors that cause the quad to initially rotate the wrong way and fly away from home, had no such problem with 12900. The arrow never points the wrong way; when the rescue starts, the quad just points the nose directly to home, and gently flies home. I put this video here to show how awful the old code was in these situations, and how grateful I am to Petr Ledvina for fixing the IMU code problems.