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

Add an alternate compass.calculate() function substituting 1 sqrt func for 4 trig funcs.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2221 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
deweibel@gmail.com 2011-05-08 18:15:29 +00:00
parent ebeb9797cf
commit a19b5a5c10
2 changed files with 46 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_Compass/Compass.cpp
View File

@ -63,26 +63,59 @@ Compass::get_declination()
} }
void void
Compass::calculate(float roll, float pitch) Compass::calculate(float roll, float pitch)
{ {
// Note - This function implementation is deprecated
// The alternate implementation of this function using the dcm matrix is preferred
float headX; float headX;
float headY; float headY;
float cos_roll; float cos_roll;
float sin_roll; float sin_roll;
float cos_pitch; float cos_pitch;
float sin_pitch; float sin_pitch;
cos_roll = cos(roll);
cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? sin_roll = sin(roll);
sin_roll = 1 - (cos_roll * cos_roll);
cos_pitch = cos(pitch); cos_pitch = cos(pitch);
sin_pitch = 1 - (cos_pitch * cos_pitch); sin_pitch = sin(pitch);
// Tilt compensated magnetic field X component: // Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch; headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch;
// Tilt compensated magnetic field Y component: // Tilt compensated magnetic field Y component:
headY = mag_y*cos_roll-mag_z*sin_roll; headY = mag_y*cos_roll - mag_z*sin_roll;
// magnetic heading
heading = atan2(-headY,headX);
// Declination correction (if supplied)
if( fabs(_declination) > 0.0 )
{
heading = heading + _declination;
if (heading > M_PI) // Angle normalization (-180 deg, 180 deg)
heading -= (2.0 * M_PI);
else if (heading < -M_PI)
heading += (2.0 * M_PI);
}
// Optimization for external DCM use. Calculate normalized components
heading_x = cos(heading);
heading_y = sin(heading);
}
void
Compass::calculate(const Matrix3f &dcm_matrix)
{
float headX;
float headY;
float cos_pitch = sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
// sin(pitch) = - dcm_matrix(3,1)
// cos(pitch)*sin(roll) = - dcm_matrix(3,2)
// cos(pitch)*cos(roll) = - dcm_matrix(3,3)
// 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;
// Tilt compensated magnetic field Y component:
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
// magnetic heading // magnetic heading
heading = atan2(-headY,headX); heading = atan2(-headY,headX);

6
libraries/AP_Compass/Compass.h
View File

@ -58,6 +58,12 @@ public:
/// ///
virtual void calculate(float roll, float pitch); virtual void calculate(float roll, float pitch);
/// Calculate the tilt-compensated heading_ variables.
///
/// @param dcm_matrix The current orientation rotation matrix
///
virtual void calculate(const Matrix3f &dcm_matrix);
/// Set the compass orientation matrix, used to correct for /// Set the compass orientation matrix, used to correct for
/// various compass mounting positions. /// various compass mounting positions.
/// ///