Automatic compass mounting-orientation detection during calibration#11708
Draft
sensei-hacker wants to merge 3 commits into
Draft
Automatic compass mounting-orientation detection during calibration#11708sensei-hacker wants to merge 3 commits into
sensei-hacker wants to merge 3 commits into
Conversation
compassUpdate() intentionally skips offset/gain correction while a calibration spin is in progress (the offsets aren't known yet), but imuCalculateEstimatedAttitude()'s canUseMAG check had no way to know calibration was active, so it fused the raw, hard-iron-biased samples into the yaw estimate for the whole spin. Promote compass.c's calStartedAt to file scope and expose it via a new compassIsCalibrating(), then gate canUseMAG on it in imu.c. During a spin, yaw now comes from gyro integration only, same as it already does when there's no compass at all. Also extract compassInit()'s alignment-cache setup (mag.dev.magAlign useExternal/externalRotation/onBoard) into compassRefreshAlignment(), callable on its own so alignment changes made after boot (e.g. by a future auto-detected orientation) can take effect immediately instead of only after a reboot. Pure refactor, no behavior change.
…ration Adds sensors/compass_orientation.c/.h: during the existing magnetometer calibration spin, buffer magnetometer samples paired with the attitude quaternion at the moment each was taken, then compare the implied earth magnetic field across 16 candidate mounting rotations (8 cardinal yaw x upright/inverted, 8 diagonal-45deg yaw x upright/inverted). The correct rotation is the one that makes the implied earth field most consistent across samples taken at different attitudes - no true-north or GPS reference needed. Confidence is the ratio of the second-best candidate's variance to the best one's; a Monte Carlo sweep across wide synthetic noise/attitude-error ranges showed accuracy is ~100% for any accepted result at confidence >= 1.2, so the fixed 2.0 threshold used here has a comfortable margin. Detection only runs, and only auto-applies, when neither the compass nor the accelerometer has ever been calibrated on this board - the clearest available signal this is a genuinely first-time setup, not a routine recalibration of an already-mounted compass. This required no new settings.yaml entries: the enable condition is computed from existing calibration state rather than a persistent flag (which would risk silently re-triggering on some future unrelated recalibration), and the confidence threshold is a fixed constant rather than user-tunable, since nobody picking a value for a statistical variance ratio has a principled basis to choose better than the analyzed default. On success, the detected rotation is written directly to the existing align_mag_roll/pitch/yaw settings and applied immediately via compassRefreshAlignment(), using the same auto-save call calibration already performs - no separate "apply" step. Also fixes an unrelated but adjacent bug found while wiring this up: compass.c intentionally skips offset/gain correction while a spin is in progress, but imu.c's canUseMAG check had no way to know that, so it was fusing the raw, hard-iron-biased samples into the yaw estimate for the whole spin - the same attitude estimate this feature depends on being bias-free. Fixed by exposing compassIsCalibrating() and gating canUseMAG on it. Adds DEBUG_MAG_CALIB for field diagnosis of the computed confidence value, and a compass_orientation_unittest.cc covering recovery of all 16 candidates from synthetic data, rejection of an ambiguous "spun flat on a table" spin, insufficient-sample rejection, noise tolerance, and buffer overflow handling.
…nce bypass A detected identity rotation (0,0,0) is indistinguishable from "unconfigured" in rollDeciDegrees/pitchDeciDegrees/yawDeciDegrees, so it was silently discarded; route that case through mag_align=CW0_DEG instead, which can represent "explicitly no rotation" unambiguously. Also guard against a stuck/disconnected axis leaving magGain at 0: dividing by it produced NaN, which bypassed the confidence-threshold rejection since NaN < threshold is always false. Add #ifdef USE_MAG guarding (matching platform.h availability) so the module compiles to nothing on mag-less targets instead of always being built. Add a regression test for the zero-gain/NaN case.
|
Test firmware build ready — commit Download firmware for PR #11708 240 targets built. Find your board's
|
This file contains hidden or 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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
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.
Summary
Adds automatic detection of compass mounting orientation, running during the
existing magnetometer calibration spin. Based on ArduPilot's
CompassCalibrator::calculate_orientation()technique (minimum variance ofthe "implied earth field" across candidate mounting rotations), adapted for
INAV's calibration flow. Also fixes a real, independently-valuable bug found
while implementing this: the AHRS filter was fusing raw, hard-iron-biased
magnetometer samples into the yaw estimate for the entire duration of a
calibration spin.
This is a draft PR — hardware testing (real calibration spins at known
mounting orientations, compared against
get align_mag_roll/pitch/yaw) isplanned as a follow-up before this is ready for review/merge.
Changes
flight/imu.c):canUseMAGnow also requires!compassIsCalibrating(). Offsets aren'tknown yet during a spin, so the uncorrected samples were leaking hard-iron
bias into the attitude estimate — the same attitude reference the new
orientation-detection feature depends on being bias-free.
compass_orientation.c/.hmodule: buffers up to 128(rawMag, attitude-quaternion) sample pairs during a calibration spin,
then searches 16 discrete candidate mounting rotations (8 cardinal +
8 diagonal, upright/inverted) for the one that minimizes variance of the
implied earth field. Returns a confidence ratio (second-best/best
variance); auto-applies only above a fixed threshold.
compassUpdate()(sensors/compass.c): auto-detectiononly runs when neither the compass nor the accelerometer has ever been
calibrated before (
!STATE(COMPASS_CALIBRATED) && !STATE(ACCELEROMETER_CALIBRATED)) — the clearest available "genuinelyfirst-time setup" signal. No new settings were needed: the confidence
threshold is a fixed compile-time constant (average users have no
principled basis to tune it better than the computed default), and the
precondition above replaces what would otherwise have been an opt-in
setting.
mag_align=CW0_DEGrather than the
rollDeciDegrees/etc. fields, since those fields useall-zero as an "unconfigured" sentinel and would silently discard a
genuine identity-rotation result.
compass_orientation_unittest.cc): recovery of all 16candidates from synthetic data, rejection of an ambiguous "flat spin"
(yaw-only, the classic calibration mistake), insufficient-sample
rejection, noise tolerance, buffer-overflow safety, and a zero-magGain
regression test (guards against a stuck/disconnected axis producing NaN
that would otherwise bypass the confidence-threshold check).
Testing
compass_orientation_unittest: 6/6 passing.make check): 136/136 passing (confirmsno regression in unrelated tests).
AIKONF7 (mag-less target, confirms the
#ifdef USE_MAGguarding doesn'tbreak non-MAG targets).
attitude error, including an off-grid tilted-mast case up to 30°) to
characterize the confidence threshold: confidence >= 1.2 corresponds to
~100% correctness; the shipped threshold of 2.0 leaves a comfortable
margin.
targets have single-precision-only FPUs — see Use single-precision math to save ~17 KB of flash on F722 targets #11402 for prior art on
this class of bug): source-level audit plus an object-code symbol check
(
arm-none-eabi-nmon the compiled MATEKF722 object) confirmed zerodouble-precision runtime helper symbols.
orientations. This is the planned next step before removing draft status.
Related Issues
None filed; this implements a firmware-side improvement identified during
research into ArduPilot's orientation-detection approach.