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: tidy up the nan checking in DCM

Browse Source 
use is_nan() on the matrix rather than just on c.x, and add
safe_asin() to the (unused) OUTPUTMODE==2 code.
This commit is contained in:
Andrew Tridgell 2012-02-24 10:45:47 +11:00
parent c8e6d03c67
commit bad653f230
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_DCM/AP_DCM.cpp
View File

@ -221,6 +221,7 @@ AP_DCM::matrix_reset(bool recover_eulers)
float cr = cos(roll); float cr = cos(roll);
float sy = sin(yaw); float sy = sin(yaw);
float cy = cos(yaw); float cy = cos(yaw);
//Serial.printf("setting DCM matrix to %f %f %f\n", ToDeg(roll), ToDeg(pitch), ToDeg(yaw));
_dcm_matrix.a.x = cp * cy; _dcm_matrix.a.x = cp * cy;
_dcm_matrix.a.y = (sr * sp * cy) - (cr * sy); _dcm_matrix.a.y = (sr * sp * cy) - (cr * sy);
_dcm_matrix.a.z = (cr * sp * cy) + (sr * sy); _dcm_matrix.a.z = (cr * sp * cy) + (sr * sy);
@ -274,7 +275,7 @@ AP_DCM::check_matrix(void)
renorm_range_count++; renorm_range_count++;
normalize(); normalize();
if (isnan(_dcm_matrix.c.x) || if (_dcm_matrix.is_nan() ||
fabs(_dcm_matrix.c.x) > 10) { fabs(_dcm_matrix.c.x) > 10) {
// normalisation didn't fix the problem! We're // normalisation didn't fix the problem! We're
// in real trouble. All we can do is reset // in real trouble. All we can do is reset
@ -479,7 +480,7 @@ AP_DCM::euler_angles(void)
#if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes)
roll = atan2(_accel_vector.y, -_accel_vector.z); // atan2(acc_y, acc_z) roll = atan2(_accel_vector.y, -_accel_vector.z); // atan2(acc_y, acc_z)
pitch = asin((_accel_vector.x) / (double)9.81); // asin(acc_x) pitch = safe_asin((_accel_vector.x) / (double)9.81); // asin(acc_x)
yaw = 0; yaw = 0;
#else #else
pitch = -safe_asin(_dcm_matrix.c.x); pitch = -safe_asin(_dcm_matrix.c.x);