From 622217357c311d62378d2d74dfa13d6d0dbafea9 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Thu, 12 Jan 2012 14:43:39 -0700 Subject: [PATCH] Bug fix for compass. This is a fix for an interesting bug when a DCM matrix reset was added to the ground start. This bug only showed up if (A) a ground start were performed after an air start or due to use of the "Calibrate Gryo" action, (B) if the current orientation were sufficiently different from 0/0/0, and (C.) if the particular magnetometer had sufficiently large offsets. Why did resetting the DCM matrix to 0/0/0 pitch/roll/yaw at ground start cause a bug? The magnetometer offset nulling determines the proper offsets for the magnetometer by comparing the observed change in the magnetic field vector with the expected change due to rotation as calculated from the rotation in the DCM matrix. This comparison is made at 10Hz, and then filtered with a weight based on the amount of rotation to estimate the offsets. Normally it would take considerable time at normal in-flight rotation rates for the offset estimate to converge. If a DCM matrix reset occurs when the offset nulling algorithm is up and running, the algorithm sees the DCM reset as a instantaneous rotation, however the magnetic field vector did not change at all. Under certain conditions the algorithm would interpret this as indicating that the offset(s) should be very large. Since the "rotation" could also have been large the filter weighting would be large and it was possible for a large erroneous estimate of the offset(s) to be made based on this single (bad) data point. To fix this bug methods were added to the compass object to start and stop the offset nulling algorithm. Further, when the algorithm is started, it is set up to get fresh samples. The DCM matrix reset method now calls these new methods to stop the offset nulling before resetting the matrix, and resume after the matrix has been reset. --- ArduCopter/sensors.pde | 1 + ArduPlane/system.pde | 1 + ArduPlane/test.pde | 1 + libraries/AP_Compass/Compass.cpp | 18 ++++++++++++++++++ libraries/AP_Compass/Compass.h | 13 ++++++++++++- libraries/AP_DCM/AP_DCM.cpp | 3 +++ 6 files changed, 36 insertions(+), 1 deletion(-) diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 9fa33a5252..21692965bd 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -81,6 +81,7 @@ static void init_compass() dcm.set_compass(&compass); compass.init(); compass.get_offsets(); // load offsets to account for airframe magnetic interference + compass.null_offsets_enable(); } static void init_optflow() diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 5b802942d6..7cced3635c 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -202,6 +202,7 @@ static void init_ardupilot() } else { dcm.set_compass(&compass); compass.get_offsets(); // load offsets to account for airframe magnetic interference + compass.null_offsets_enable(); } } #endif diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index f55604454e..ebe7d76091 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -586,6 +586,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) Serial.println_P(PSTR("Compass initialisation failed!")); return 0; } + compass.null_offsets_enable(); dcm.set_compass(&compass); report_compass(); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index d67c900ed0..4d5d69a9c4 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -11,6 +11,7 @@ Compass::Compass(AP_Var::Key key) : _offset (&_group, 1), _declination (&_group, 2, 0.0, PSTR("DEC")), _null_init_done(false), + _null_enable(false), product_id(AP_COMPASS_TYPE_UNKNOWN) { // Default the orientation matrix to none - will be overridden at group load time @@ -146,6 +147,8 @@ Compass::null_offsets(const Matrix3f &dcm_matrix) float weight; Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z); + + if(_null_enable == false) return; if(_null_init_done) { dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix; // Note 11/20/2010: transpose() is not working, transposed() is. @@ -166,3 +169,18 @@ Compass::null_offsets(const Matrix3f &dcm_matrix) _last_dcm_matrix = dcm_matrix; } + + +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; +} \ No newline at end of file diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index a2550f2518..33f5ccafbb 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -41,7 +41,7 @@ public: float heading_x; ///< compass vector X magnitude float heading_y; ///< compass vector Y magnitude unsigned long last_update; ///< millis() time of last update - bool healthy; ///< true if last read OK + bool healthy; ///< true if last read OK /// Constructor /// @@ -117,6 +117,16 @@ public: void null_offsets(const Matrix3f &dcm_matrix); + /// Enable/Start automatic offset updates + /// + void null_offsets_enable(void); + + + /// Disable/Stop automatic offset updates + /// + void null_offsets_disable(void); + + /// Sets the local magnetic field declination. /// /// @param radians Local field declination. @@ -130,6 +140,7 @@ protected: AP_VarS _offset; AP_Float _declination; + bool _null_enable; ///< enabled flag for offset nulling bool _null_init_done; ///< first-time-around flag used by offset nulling Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling Vector3f _mag_body_last; ///< ?? used by offset nulling diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index ceedb1055f..fd201551c4 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -195,6 +195,7 @@ AP_DCM::accel_adjust(void) void AP_DCM::matrix_reset(void) { + _compass->null_offsets_disable(); _dcm_matrix.a.x = 1.0f; _dcm_matrix.a.y = 0.0f; _dcm_matrix.a.z = 0.0f; @@ -207,6 +208,8 @@ AP_DCM::matrix_reset(void) _omega_I.x = 0.0f; _omega_I.y = 0.0f; _omega_I.z = 0.0f; + _compass->null_offsets_enable(); // This call is needed to restart the nulling + // Otherwise the reset in the DCM matrix can mess up the nulling } /*************************************************