From e4d28b12e52de458feb907014a542b07578b8839 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 20 Jun 2012 19:30:30 +1000 Subject: [PATCH] Compass: remove the need to call calculate() on the compass object the new AHRS code doesn't use calculate() and the compass.heading attribute. Instead it works on the raw magnetometer vector. This change removes the internal calculate state from the compass object and instead adds calculate_heading() for use by older code that doesn't go via AHRS. This significantly reduces the calculation involved in compass updates The null offsets enable/disable code is also removed, as it is not needed now that compass offsets are not linked to the AHRS state. --- libraries/AP_Compass/Compass.cpp | 56 ++++--------------- libraries/AP_Compass/Compass.h | 22 ++------ .../AP_Compass_test/AP_Compass_test.pde | 5 +- 3 files changed, 21 insertions(+), 62 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index ddbcf93f81..7a96b6cf76 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -22,7 +22,6 @@ Compass::Compass(void) : _learn(1), _use_for_yaw(1), _auto_declination(1), - _null_enable(false), _null_init_done(false) { } @@ -66,9 +65,7 @@ Compass::set_initial_location(long latitude, long longitude) // the declination based on the initial GPS fix if (_auto_declination) { // Set the declination based on the lat/lng from GPS - null_offsets_disable(); _declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000))); - null_offsets_enable(); } } @@ -85,8 +82,8 @@ Compass::get_declination() } -void -Compass::calculate(float roll, float pitch) +float +Compass::calculate_heading(float roll, float pitch) { // Note - This function implementation is deprecated // The alternate implementation of this function using the dcm matrix is preferred @@ -96,6 +93,8 @@ Compass::calculate(float roll, float pitch) float sin_roll; float cos_pitch; float sin_pitch; + float heading; + cos_roll = cos(roll); sin_roll = sin(roll); cos_pitch = cos(pitch); @@ -118,18 +117,18 @@ Compass::calculate(float roll, float pitch) heading += (2.0 * M_PI); } - // Optimization for external DCM use. Calculate normalized components - heading_x = cos(heading); - heading_y = sin(heading); + return heading; } -void -Compass::calculate(const Matrix3f &dcm_matrix) +float +Compass::calculate_heading(const Matrix3f &dcm_matrix) { float headX; float headY; float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x)); + float heading; + // sin(pitch) = - dcm_matrix(3,1) // cos(pitch)*sin(roll) = - dcm_matrix(3,2) // cos(pitch)*cos(roll) = - dcm_matrix(3,3) @@ -138,7 +137,7 @@ Compass::calculate(const Matrix3f &dcm_matrix) // 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; + return 0; } // Tilt compensated magnetic field X component: @@ -159,23 +158,7 @@ Compass::calculate(const Matrix3f &dcm_matrix) heading += (2.0 * M_PI); } - // Optimization for external DCM use. Calculate normalized components - heading_x = cos(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 + return heading; } @@ -202,7 +185,7 @@ Compass::calculate(const Matrix3f &dcm_matrix) void Compass::null_offsets(void) { - if (_null_enable == false || _learn == 0) { + if (_learn == 0) { // auto-calibration is disabled return; } @@ -274,18 +257,3 @@ Compass::null_offsets(void) // set the new offsets _offset.set(_offset.get() - diff); } - - -void -Compass::null_offsets_enable(void) -{ - _null_init_done = false; - _null_enable = true; -} - -void -Compass::null_offsets_disable(void) -{ - _null_init_done = false; - _null_enable = false; -} diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 8adc3ede13..f02f78c833 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -20,9 +20,6 @@ public: int16_t mag_x; ///< magnetic field strength along the X axis int16_t mag_y; ///< magnetic field strength along the Y axis int16_t mag_z; ///< magnetic field strength along the Z axis - float heading; ///< compass heading in radians - float heading_x; ///< compass vector X magnitude - float heading_y; ///< compass vector Y magnitude uint32_t last_update; ///< micros() time of last update bool healthy; ///< true if last read OK @@ -46,13 +43,17 @@ public: /// @param roll The current airframe roll angle. /// @param pitch The current airframe pitch angle. /// - virtual void calculate(float roll, float pitch); + /// @returns heading in radians + /// + virtual float calculate_heading(float roll, float pitch); /// Calculate the tilt-compensated heading_ variables. /// /// @param dcm_matrix The current orientation rotation matrix + /// + /// @returns heading in radians /// - virtual void calculate(const Matrix3f &dcm_matrix); + virtual float calculate_heading(const Matrix3f &dcm_matrix); /// Set the compass orientation matrix, used to correct for /// various compass mounting positions. @@ -100,16 +101,6 @@ public: /// void null_offsets(void); - - /// Enable/Start automatic offset updates - /// - void null_offsets_enable(void); - - - /// Disable/Stop automatic offset updates - /// - void null_offsets_disable(void); - /// return true if the compass should be used for yaw calculations bool use_for_yaw(void) { return healthy && _use_for_yaw; } @@ -130,7 +121,6 @@ protected: AP_Int8 _use_for_yaw; ///