Increase IMU yaw CoG gain at higher positive pitch angles #12791
Conversation
This comment has been minimized.
This comment has been minimized.
|
Note - this is built over the changes in the bugfix PR #12789 |
ctzsnooze
force-pushed
the
PR-GPS-Rescue-Pitch-factor-for-yaw-IMU-gain
branch
from
95ed09c
to
f1d5005
Compare
This comment has been minimized.
This comment has been minimized.
|
AUTOMERGE: (FAIL)
|
|
@ctzsnooze : Can you test with #12792? |
|
There is another possibility, by modifying IMU code. Instead of assuming forward flight (projecting X axis to ground and comparing it to ground course), project Z (up) axis instead.
It won't work for plane mode (can be switched conditionally; and plane mode is poorly supported, if at all). It may cause problem on quads with pre-tilted propellers (can be fixed by projecting propeller vector instead of Z axis). |
|
Do you want to test this code? Here you have an automated build: |
|
Today I had time to test this PR, in particular checking the situation where the quad had a 180 degree IMU error and was not drifting much. The quad pitches forward quickly, away from home, due to the velocity to home error, but takes time to accumulate groundspeed. Because of the rapid pitch angle change, the quad is very quick to re-orient the IMU, whereas before it would need to fly some 50-60m away before the speed error was enough to get IMU re-orientation. From any angle and with any drift that I tested, with or without IMU error, and it returned home effectively. Sometimes the initial correction as it gained speed caused an overshoot on yaw, but the overshoot kind of flew the quad back towards what would have been to the intended track if the IMU had been correct. It worked very well. I made an interesting finding when I tested this code with PR #12715, which allows Rescue to enter Descent mode when initiated within our old 30m limit. When I accidentally set the IMU gain to zero, and initiated soon after takeoff, the IMU wasn't yet synced to the ground course, and the quad travelled along an incorrect course, thinking it was going to home, and never corrected at all. It just went straight. With normal IMU gain the correction was slow at first; with this PR, the quad turned very quickly, and came home with surprising immediacy and accuracy. This strongly suggests that a more general application of the idea within the IMU code would be a good idea. Although the IMU may not always travel entirely in the direction of the pitch or roll angle, it doesn't take long before most of its speed is derived from the dominant angle of the quad. @ledvinap - I apologise for not testing your PR #12792 yet, I will do that as soon as the weather permits. |
|
@ctzsnooze: Maybe test #12792 (comment) (it is quite possible that it will orient heading with 90/180 deg offset, so be careful in RTH). It will use quad direction implicitly (with less gain) both in RTH and normal mode. The gain may be adjusted if it is problematic, possibly also applying some deadband around level position. |
|
Closing this PR as test results were very favourable for in-close rescues. |
@ledvinap encouraged consideration of a pitch angle based factor to dynamically modulate the IMU gain for yaw.
This PR implements this in a simple way during a GPS rescue. It adds to the recent PR where we modulated the IMU gain based on groundspeed error. In this PR we add another modulation factor based on pitch angle.
In this PR, when the pitch angle is positive, which normally would be associated with forward flight, the pitch angle in degrees divided by 10 is added to the IMU gain multiplier. For example, if the quad pitched forward the full 45 degrees, we add 4.5 to the IMU gain value, bringing it close to the maximum allowed 6x gain. Any IMU error should then correct more quickly when pitched forward.
This will be very useful when a rescue is initiated with minimal drift and a 180 degree IMU error. In this case, there is little or no groundspeed error at the start, but a significant velocity to home error. The lack of a groundspeed error means that we almost immediately get a strong pitch forward in the completely wrong direction. However it still takes 1-2 seconds before we get enough speed to boost the IMU gain enough to correct the error. The quad can cover some 50-60m away from home in this time. With this pitch based factor we should much more quickly get the desired IMU gain boost, because the pitch angle change precedes the groundspeed error factor. The flyaway should be attenuated considerably.
In other common rescue situations, the drift factor will already be pushing the IMU gain factor high, so it won't help as much. It is not likely to do any harm because while pitched forward, especially over time, we typically get a velocity over ground that is 'nose-forward' and appropriate for IMU correction.
It should prevent IMU error accumulation when the quad is pitched backwards, as can sometimes happen in a rescue.
This approach could be applied more generally. In non-GPS rescue flight, it is mostly true that the IMU is more likely to be correct when we are flying forwards, and that we are more likely to be flying forwards when we are requesting a positive pitch angle. This is not always the case, however. For example, the presence of strong side-winds, or being only slightly pitched forward while carrying significant prior sideways or rear-wards drift, or when there is a significant roll element, we may have a vector over ground that is not entirely nose-forward, and could even be be nose-backward. Additionally, it takes time for a pitch forward attitude to generate forward speed, and also it takes time for that forward speed to 'bleed off'. Hence a 'general' solution for IMU error would need to be implemented cautiously, perhaps using smoothing so that we respond primarily to a sustained pitch forward effect and not to transients, and probably considering the roll angle as well. It would be great to have a more general solution, since then may be able to prevent many common IMU errors from developing.
However, in the GPS Rescue context, this PR is likely to help minimise the impact of common pre-existing IMU errors at the time a rescue starts. Despite being a simple heuristic solution, we do need a short-term fix, especially for low-drift Rescue initiations with 180 degree pre-existing IMU errors.