From 7cc2382a78471a2c6271b47c7fe863e4e3f11ede Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Fri, 15 Apr 2022 19:59:48 -0400 Subject: [PATCH] AP_AHRS_DCM: NFC comment on check_matrix() normalization reset --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 7ffd3c7545..6f447fba80 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -265,7 +265,10 @@ AP_AHRS_DCM::check_matrix(void) normalize(); if (_dcm_matrix.is_nan() || - fabsf(_dcm_matrix.c.x) > 10) { + fabsf(_dcm_matrix.c.x) > 10.0) { + // See Issue #20284: regarding the selection of 10.0 for DCM reset + // This won't be lowered without evidence of an issue or mathematical proof & testing of a lower bound + // normalisation didn't fix the problem! We're // in real trouble. All we can do is reset //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",