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

Compass: ensure we don't produce NAN values for compass heading

Browse Source 
this leaves the previous heading values alone if we are at a pitch of
exactly 90 or -90, at which point we can't compute a meaningful
heading
This commit is contained in:
Andrew Tridgell 2012-02-24 10:42:03 +11:00
parent 8190204287
commit 50f85835bd
1 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Compass/Compass.cpp
View File

@ -114,11 +114,18 @@ Compass::calculate(const Matrix3f &dcm_matrix)
{ {
float headX; float headX;
float headY; float headY;
float cos_pitch = sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x)); float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
// sin(pitch) = - dcm_matrix(3,1) // sin(pitch) = - dcm_matrix(3,1)
// cos(pitch)*sin(roll) = - dcm_matrix(3,2) // cos(pitch)*sin(roll) = - dcm_matrix(3,2)
// cos(pitch)*cos(roll) = - dcm_matrix(3,3) // cos(pitch)*cos(roll) = - dcm_matrix(3,3)
if (cos_pitch == 0.0) {
// we are pointing straight up or down so don't update our
// heading using the compass. Wait for the next iteration when
// we hopefully will have valid values again.
return;
}
// Tilt compensated magnetic field X component: // Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch; headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch;
// Tilt compensated magnetic field Y component: // Tilt compensated magnetic field Y component:
@ -140,6 +147,20 @@ Compass::calculate(const Matrix3f &dcm_matrix)
// Optimization for external DCM use. Calculate normalized components // Optimization for external DCM use. Calculate normalized components
heading_x = cos(heading); heading_x = cos(heading);
heading_y = sin(heading); heading_y = sin(heading);
#if 0
if (isnan(heading_x) || isnan(heading_y)) {
Serial.printf("COMPASS: c.x %f c.y %f c.z %f cos_pitch %f mag_x %d mag_y %d mag_z %d headX %f headY %f heading %f heading_x %f heading_y %f\n",
dcm_matrix.c.x,
dcm_matrix.c.y,
dcm_matrix.c.x,
cos_pitch,
(int)mag_x, (int)mag_y, (int)mag_z,
headX, headY,
heading,
heading_x, heading_y);
}
#endif
} }
void void