Improve GPS Rescue Pitch smoothing and disarming

[ctzsnooze]

Note: there is sampling and aliasing issue in our 4.4 estimate of GPS data time intervals. It caused most of the noise in our velocity P and D data, and most of the pitch stepping problems that people complained about.
This PR includes a simple, temporary, 'hack', which locks the gps data interval to the nearest likely GPS data rate based on our measurement of the rate. This greatly reduces pitch axis shaking and allows much lower filters, which mean lower PIDs.
A better method is needed and is being worked on by @KarateBrot . In the meantime, having solid intervals means solid velocity data with less filter delay, and a far better rescue experience.

User benefits of this PR:

• safety: immediate disarm on initiation if rescue switch is bumped while armed and within 5m of home
• much smoother pitch control during the rescue
• user-adjustable pitch smoothing with gps_rescue_pitch_cutoff, can be very smooth or very fast
• much more reliable disarm on landing
• much less likely to disarm just above the ground
• user-adjustable gps_rescue_disarm_threshold to raise threshold if a noisy build tends to disarm early
• dynamic adjustment of pitch smoothing depending on phase of flight
• more accurate velocity control during the rescue
• faster climb and return by default
• higher default max angle to be sure we can return even against a strong wind
• bug fixe for issue where initial altitude was higher than set return value

Changes:

• no longer increases the disarm sensitivity depending on rate of altitude change, to reduce chance of false disarm
• subtract 1G from Z axis Accelerometer reading, so that when all accelerometer values are 'normal', the accelerometer signal that gets compared to the disarm threshold is 0. Previously it was always 1.0 due to the effect of gravity on the Z axis. This meant that only a 1G positive change in Z axis acc reading was enough to trigger disarm, possibly a false disarm, while a negative 3G change was needed to disarm (perhaps failing to detecting bouncing). Now a 2G sum change is needed, with the Z axis handling positive and negative changes from normal correctly.
• Disarm threshold is user-adjustable, using the CLI gps_rescue_disarm_threshold value. Default is 20, as before. A lower value will disarm more readily, and a higher value may cause the auto-disarm-detection to fail. If the quad does not disarm on landing, it will time-out and disarm after 10s, so this isn't a big problem.
  -User-adjustable pitch smoothing using gps_rescue_pitch_cutoff in CLI. Default is 100 or 1.0Hz. Larger values make the velocity control more precise, but pitch angle will jitter more. Lower values, eg 50, make the pitch changes smoother, but will delay velocity control and may cause pitch to wobble slowly or take time to settle. Higher values may be useful for very high speed returns where lag is unhelpful.
• switch from PT3 back to PT1 on velocity D, at the gps_rescue_pitch_cutoff frequency, and remove the 2 point moving average. This is possible now that the velocity data is better. With less noise and less delay, default D value can be set lower and still be very effective.
• default velocity D factor now only 12 to minimise noise and wobble.
• use PT3 for velocity upsampling at 4x the gps_rescue_pitch_cutoff cutoff, which gives a fairly good compromise between smoothness and lag.
• dynamic adjustment of the cutoff value. At the start of the pitch forward, when less delay is important, the cutoff is doubled, and smoothly eases back to the set value with a time constant of two seconds. On descent, the cutoff increases as we get lower, to ensure accurate control in the terminal phases.
• smoother acquisition of the velocity target value at the start by exponentially fading the target value in, with a time constant of one second. This avoids the hard pull-back that sometimes happened once velocity was acquired.
• velocity iTerm relax factor, with a time constant of 2s from the start of the pitch forward, to prevent iTerm growth in the first second or two, when there is unavoidable velocity error.
• increase default return speed 5.00 to 7.50 m/s, since many users want to home more quickly
• increase default climb rate from 5.00 to 7.50 m/s, since many users want to get up quickly
• increase default descend rate from 1.00 to 1.50 m/s, since a slightly faster descent gives a more solid disarm signal, and to reduce descent time
• increase default max return angle to 70 degrees, to support faster returns especially against strong winds
• re-name gps_rescue_pitch_angle_max to gps_rescue_max_angle
• some small rescue PID settings changes
• fixed a bug where the altitude control start indicator was not static
• clarified old comments and added new ones

Setting gps_rescue_pitch_cutoff

For faster than default rescues, eg more than 10m/s, don't set gps_rescue_pitch_cutoffvalue below default. Too much smoothing (lower cutoff values) will cause lag that may cause pitch wobble. Most likely the cutoff will need to be increased, eg to 200.

For greater smoothness, and a slightly 'vague' or 'delayed' feel to the return, try set gps_rescue_pitch_cutoff = 50`. and perhaps choose a return velocity that is a little bit lower.

Choose a smoothness value that suits your needs. The default should be good for most purposes.

Note that when higher levels of smoothness are required, overall precision will become worse, and on windy days, some corrections may be slower than optimal. When the smoothing is too strong, the quad will wallow around, or wobble, especially at the start, or on windy days.

If the GPS unit has an unstable position value, with a low sat count, or is constantly gaining or dropping satellites, the quad may abruptly jump around during a return. This is not due to a code problem, but to erratic data from the GPS.

Setting gps_rescue_disarm_threshold

• There is a simple tradeoff here; if the gps_rescue_disarm_threshold is high, false disarms while in the final landing phase will be less likely, but a gentle landing may not have enough impact energy to reliably disarm the quad.
• If the auto-disarm fails, the motors will keep spinning for about 10s, and then disarm, rather than disarming immediately.
• It's probably better to accept some landings that don't disarm, than have it disarm several meters above hard ground.
• If the user has modified their acc_lpf_hz value, this will affect the optimal disarm threshold value. In unusual cases the cutoff could be lowered (increase smoothing) if there is a lot of noise on the accelerometer. However, very low cutoffs may smooth out the disarm signal itself, preventing reliable disarm on true impacts.

Increase the gps_rescue_disarm_threshold to say 25 or 30 (2.5G or 3G) if:

• the quad is fragile and expensive, and the landing surface hard, or
• the quad is noisy, or
• the quad is often flown in very windy days, or
• the quad has experienced random disarms in the air, during landing phase

Decrease the gps_rescue_disarm_threshold if:

• the quad frequently fails to automatically disarm on landing
• the landing surface is soft
• you really want it to disarm automatically every time

Limitations / cautions

• if the pilot sets very high return velocities, gps_rescue_max_angle and gps_rescue_pitch_cutoff should not be any lower than default. The quad will need to quickly adjust the pitch angle, perhaps to very high angles, at times. Expect to see rapid and substantial adjustments in pitch angle.

Other technical changes

• re-name the internal code representation of max rescue angle from what was angle to maxRescueAngle, to make it much easier to find this value in the code
• in the gps_rescue code, rename sampleTimeS to gpsRescueTaskIntervalSeconds for clarity
• make sure that timers for smoothing things in the gps_rescue sensorUpdate() function use the gpsRescueTaskIntervalSeconds value, not 0.01f
• remove the GPS_verticalSpeedInCmS code, which is not used anymore
• get the gps data interval to use in gps_rescue.c from gps.c via getGpsDataIntervalSeconds()
• GPS_RESCUE_MAX_PITCH_RATE is removed because it not used to smooth velocity data
• GPS_RESCUE_MAX_ITERM_VELOCITY is removed because the velocity iTerm is now limited to half the max user-configured angle (half the velocity pidSum limit).
• consistently re-name the failsafe switch booleans eg boxFailsafeSwitchWasOn, to make them easier to find when searching for boxfailsafe.
• resolve some issues relating to GPS_RESCUE MODE, including ensuring reliable exit with sticks.
• remove unnecessary failsafeOnValidDataFailed() in failsafe.c; this had previously been flagged as likely not necessary, and testing shows it is not.
• remove the check on arming status in rx.c detectAndApplySignalLossBehaviour. This would have had the effect of not checking Rx packets for validity while disarmed, which doesn't seem sensible. The failsafe code handles failsafe behaviour in relation to arm status.
• remove GPS_verticalSpeedInCmS since not used and duplicated the various function

[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 -> 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

[druckgott]

* increase default descend rate from 100 to 150 m/s, since a slightly faster descent gives a more solid disarm signal, and in any case some descents take a long time

Is this realy 150 m/s --> 540 km/h descent rate? (maybe 10-15 m/s?)

[characharm]

* increase default descend rate from 100 to 150 m/s, since a slightly faster descent gives a more solid disarm signal, and in any case some descents take a long time

Is this realy 150 m/s --> 540 km/h descent rate? (maybe 10-15 m/s?)

.descendRate = 150,         // cm/s, minimum for descent and landing phase, or for descending if starting high ascent

[ctzsnooze]

* increase default descend rate from 100 to 150 m/s, since a slightly faster descent gives a more solid disarm signal, and in any case some descents take a long time

Is this realy 150 m/s --> 540 km/h descent rate? (maybe 10-15 m/s?)

Sorry, that was a typo, fixed.

I should have said 1.00 -> 1.50 m/s descent rate (100 to 150 in CLI, cm/s), as pointed out above.

Note that this is the terminal descent rate when close to the ground, the descent rate at higher altitude can be up to 3x quicker.

[andre-hilgersom]

I installed betaflight_4.5.0_STM32F7X2_f0acc7c.hex, with option gps in the configurator enabled. But the feature gps cannot be enabled in the cli or config tab. A gps is configured in the ports tab.
Am I missing something ?

[ctzsnooze]

Tested - works well. Much smoother.

100% reliable disarms whereas before I had a small percentage too early.Default of 7.5m/s (26kph) is nice and steady.

I've tested returns at 15m/s (54kph) and at 22m/s (80kph) into 30kph headwind, no problem. For these I increased the filter cutoff to 200 to ensure brisk responses; was surprisingly smooth. If you do go fast, please give some distance for the quad to slow down in. Say 50m for 15m/s or 75m for 22m/s. Otherwise its easy to overshoot home and have to circle around, which gets scrappy. The code will pull about 1-1.5G's to slow down.

[KarateBrot]

Suggested change

	rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z] - acc.dev.acc_1G) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
	// Warning: this line assumes that the gravity vector is perfectly aligned with the attitude vector at all times
	rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z] - acc.dev.acc_1G) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;

This change looks dangerous (as in "mathematically dangerous" 🙂). It assumes that the quad is perfectly aligned with the horizon at all times. Since the quad should be pretty much level in the landing phase this is a somewhat good assumption. But at least there should be a comment that is warning about this assumption, imo.

[Quick-Flash]

Better would just be to calculate the true non gravity acceleration that the drone is experiencing. That will be used to fuse gps and acc anyhow

[ctzsnooze]

Good point. I'll add a warning.
This works much more reliably than the previous 'assumptions' :-)
I agree that a value that provided non-gravity acceleration would be much better; it would be great if that can be provided at some point in the future.
If, during landing phase the quad is on a steeper angle, that's likely part of an awkard landing, or it is rolling over, and if that outcome in the existing code results in a greater sensitivity to disarm, this 'peculiarity' is, of itself, not so bad.

[limonspb]

Did testing today for this PR, everything seems to be ok.
Maybe need to do something with the jumping from the ground (end of video)
https://youtu.be/0weHCToij9E

[OlegSyr]

Hi ctzsnooze!
Thanks for returning back to this issue!
I did testing on my 5in in stormy weather and can confirm that #12343 really improves pitch on return phase.
Changed pitch-cutoff from 50 up to 200 to understand how cutoff frequency work. Default 100 is very good.
Please let me know if you need logs or any more testing need to be done.

[github-actions]

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!

Fixed