From 8c76c8c936938d1985bfb6d7a8499a55ce507e06 Mon Sep 17 00:00:00 2001 From: deweibel Date: Tue, 8 Feb 2011 20:17:16 +0000 Subject: [PATCH] Add code for auto magnetometer offset nulling. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1615 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 70 ++++++++++++++++----- libraries/AP_Compass/AP_Compass_HMC5843.h | 7 ++- 2 files changed, 59 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 150978d18a..688a65c7cc 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -56,9 +56,10 @@ AP_Compass_HMC5843::AP_Compass_HMC5843() : orientation(0), declination(0.0) { // mag x y z offset initialisation - offset[0] = 0; - offset[1] = 0; - offset[2] = 0; + + _offset.x = 0; + _offset.y = 0; + _offset.z = 0; // initialise orientation matrix orientation_matrix = ROTATION_NONE; @@ -70,6 +71,7 @@ bool AP_Compass_HMC5843::init(int initialise_wire_lib) unsigned long currentTime = millis(); // record current time int numAttempts = 0; int success = 0; + first_call = 1; if( initialise_wire_lib != 0 ) Wire.begin(); @@ -143,6 +145,7 @@ void AP_Compass_HMC5843::read() { int i = 0; byte buff[6]; + Vector3f rot_mag; Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(0x03); //sends address to read from @@ -164,6 +167,15 @@ void AP_Compass_HMC5843::read() mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis last_update = millis(); // record time of update + // rotate the magnetometer values depending upon orientation + if( orientation == 0 ) + rot_mag = Vector3f(mag_x,mag_y,mag_z); + else + rot_mag = orientation_matrix*Vector3f(mag_x,mag_y,mag_z); + rot_mag = rot_mag + _offset; + mag_x = rot_mag.x; + mag_y = rot_mag.y; + mag_z = rot_mag.z; } } @@ -175,23 +187,16 @@ void AP_Compass_HMC5843::calculate(float roll, float pitch) float sin_roll; float cos_pitch; float sin_pitch; - Vector3f rotmagVec; 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); - sin_pitch = sin(pitch); - - // rotate the magnetometer values depending upon orientation - if( orientation == 0 ) - rotmagVec = Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]); - else - rotmagVec = orientation_matrix*Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]); + sin_pitch = 1 - (cos_pitch * cos_pitch); // Tilt compensated magnetic field X component: - headX = rotmagVec.x*cos_pitch+rotmagVec.y*sin_roll*sin_pitch+rotmagVec.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 = rotmagVec.y*cos_roll-rotmagVec.z*sin_roll; + headY = mag_y*cos_roll-mag_z*sin_roll; // magnetic heading heading = atan2(-headY,headX); @@ -210,6 +215,37 @@ void AP_Compass_HMC5843::calculate(float roll, float pitch) heading_y = sin(heading); } + +void AP_Compass_HMC5843::null_offsets(Matrix3f dcm_matrix) +{ + // Update our estimate of the offsets in the magnetometer + Vector3f calc(0.0, 0.0, 0.0); + Matrix3f dcm_new_from_last; + float weight; + + Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z); + + if(!first_call) { + dcm_new_from_last = dcm_matrix.transposed() * last_dcm_matrix; // Note 11/20/2010: transpose() is not working, transposed() is. + + weight = 3.0 - fabs(dcm_new_from_last.a.x) - fabs(dcm_new_from_last.b.y) - fabs(dcm_new_from_last.c.z); + if (weight > .001) { + calc = mag_body_new + mag_body_last; // Eq 11 from Bill P's paper + calc -= dcm_new_from_last * mag_body_last; + calc -= dcm_new_from_last.transposed() * mag_body_new; + if(weight > 0.5) weight = 0.5; + calc = calc * (weight); + _offset -= calc; + } + + } else { + first_call = 0; + } + mag_body_last = mag_body_new - calc; + last_dcm_matrix = dcm_matrix; + +} + void AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix) { orientation_matrix = rotation_matrix; @@ -221,9 +257,9 @@ void AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix) void AP_Compass_HMC5843::set_offsets(int x, int y, int z) { - offset[0] = x; - offset[1] = y; - offset[2] = z; + _offset.x = x; + _offset.y = y; + _offset.z = z; } void AP_Compass_HMC5843::set_declination(float radians) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 0364b5c5a4..7fb91f0e74 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -63,16 +63,21 @@ class AP_Compass_HMC5843 : public Compass private: int orientation; Matrix3f orientation_matrix; + Matrix3f last_dcm_matrix; + Vector3f mag_body_last; + bool first_call; float calibration[3]; - int offset[3]; + Vector3f _offset; float declination; public: AP_Compass_HMC5843(); // Constructor bool init(int initialiseWireLib = 1); void read(); void calculate(float roll, float pitch); + void null_offsets(const Matrix3f dcm_matrix); void set_orientation(const Matrix3f &rotation_matrix); void set_offsets(int x, int y, int z); + Vector3f get_offsets() {return _offset;}; void set_declination(float radians); }; #endif