mag_control: fuse fake heading during leveling fine alignment #17964
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
dagar
reviewed
Jul 23, 2021
bresch
force-pushed
the
pr-tilt-align-fake-heading-fuse-upstream
branch
from
698c2aa
to
6ce3a39
Compare
|
backport to v1.12 (using ECL) PX4/PX4-ECL#1029 |
bresch
force-pushed
the
pr-tilt-align-fake-heading-fuse-upstream
branch
from
6ce3a39
to
0ed3074
Compare
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
I recently centralized the mag reset call in
mag_control.cppand disabled the mag fusion during the leveling fine alignment phase (a685987#diff-4e4b2b3cba938a36a704d2d452e4ca2ae427d40254a8c63a5278b0ded821754cR70). This was in theory the correct way of doing it, except that without any heading fusion while fusing fake position (quasi-stationary alignment), the quaternion variance isn't reducing fast enough.This PR enables fake "zero innovation" heading fusion during the fine alignment phase to speed up the quaternion convergence.
Also enables fake heading fusion when the mag has been rejected (
mag_faultflag in case of yaw emergency reset for example) to avoid unconstrained yaw variance growth in case of long static (i.e.: no acceleration) flight, where the heading isn't observable anymore.Tested on a drone where the fine alignment phase wasn't able to complete anymore (triggered by the
at_restflag never being true; due to another unrelated issue).Note that the issue is not showing up on most setups, because when the vehicle is detected "at rest", the fake position fusion has a lower observation noise that allows the quaternion to converge much faster.