Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

DCM: changed the sense of floating point range comparison

Browse Source 
This allows us to detect NaN, otherwise NaN values were considered 
'in range'
This commit is contained in:
Andrew Tridgell 2012-02-23 11:38:51 +11:00
parent 9caa4aeb44
commit 8707965c15
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_DCM/AP_DCM.cpp
View File

@ -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