From a19b5a5c10f67783e37500597b121fe787910f5f Mon Sep 17 00:00:00 2001 From: "deweibel@gmail.com" Date: Sun, 8 May 2011 18:15:29 +0000 Subject: [PATCH] Add an alternate compass.calculate() function substituting 1 sqrt func for 4 trig funcs. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2221 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Compass/Compass.cpp | 47 +++++++++++++++++++++++++++----- libraries/AP_Compass/Compass.h | 6 ++++ 2 files changed, 46 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index fd56cc36be..54906ecd58 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -63,26 +63,59 @@ Compass::get_declination() } - void 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 headY; float cos_roll; float sin_roll; float cos_pitch; float sin_pitch; - - cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? - sin_roll = 1 - (cos_roll * cos_roll); + cos_roll = cos(roll); + sin_roll = sin(roll); cos_pitch = cos(pitch); - sin_pitch = 1 - (cos_pitch * cos_pitch); + sin_pitch = sin(pitch); // 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: - 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 heading = atan2(-headY,headX); diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index da1904fa8d..9a5557211b 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -58,6 +58,12 @@ public: /// 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 /// various compass mounting positions. ///