From 8707965c15abddd004a97c1ec01775630675d087 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 23 Feb 2012 11:38:51 +1100 Subject: [PATCH] DCM: changed the sense of floating point range comparison This allows us to detect NaN, otherwise NaN values were considered 'in range' --- libraries/AP_DCM/AP_DCM.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index e6b2e87f60..aa1a1380a0 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -235,13 +235,13 @@ AP_DCM::check_matrix(void) // the pitch calculation via asin(). These NaN values can // feed back into the rest of the DCM matrix via the // error_course value. - if (_dcm_matrix.c.x > 1.0 || - _dcm_matrix.c.x < -1.0) { + if (!(_dcm_matrix.c.x < 1.0 && + _dcm_matrix.c.x > -1.0)) { // We have an invalid matrix. Force a normalisation. renorm_range_count++; normalize(); - if (_dcm_matrix.c.x > 1.0 || - _dcm_matrix.c.x < -1.0) { + if (!(_dcm_matrix.c.x < 1.0 && + _dcm_matrix.c.x > -1.0)) { // normalisation didn't fix the problem! We're // in real trouble. All we can do is reset SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", @@ -314,10 +314,10 @@ AP_DCM::renorm(Vector3f const &a, int &problem) renorm_val = 1.0 / sqrt(a * a); - if (renorm_val > 2.0 || renorm_val < 0.5) { + if (!(renorm_val < 2.0 && renorm_val > 0.5)) { // this is larger than it should get - log it as a warning renorm_range_count++; - if (renorm_val > 1.0e6 || renorm_val < 1.0e-6) { + if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) { // we are getting values which are way out of // range, we will reset the matrix and hope we // can recover our attitude using drift