diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 66409a2afa..a23523614d 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -11,121 +11,15 @@ #include "AP_Compass_HIL.h" -// Constructors //////////////////////////////////////////////////////////////// -AP_Compass_HIL::AP_Compass_HIL() : orientation(0), declination(0.0) -{ - // mag x y z offset initialisation - memset(offset, 0, sizeof(offset)); - - // initialise orientation matrix - orientation_matrix = ROTATION_NONE; -} - // Public Methods ////////////////////////////////////////////////////////////// -bool AP_Compass_HIL::init(int initialise_wire_lib) -{ - unsigned long currentTime = millis(); // record current time - int numAttempts = 0; - int success = 0; - - // calibration initialisation - calibration[0] = 1.0; - calibration[1] = 1.0; - calibration[2] = 1.0; - - while( success == 0 && numAttempts < 5 ) - { - // record number of attempts at initialisation - numAttempts++; - - // read values from the compass - read(); - delay(10); - // calibrate - if( abs(mag_x) > 500 && abs(mag_x) < 1000 && abs(mag_y) > 500 && abs(mag_y) < 1000 && abs(mag_z) > 500 && abs(mag_z) < 1000) - { - calibration[0] = fabs(715.0 / mag_x); - calibration[1] = fabs(715.0 / mag_y); - calibration[2] = fabs(715.0 / mag_z); - - // mark success - success = 1; - } - } - return(success); -} - -// Read Sensor data void AP_Compass_HIL::read() { // values set by setHIL function } -void AP_Compass_HIL::calculate(float roll, float pitch) -{ - float headX; - float headY; - float cos_roll; - 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); - 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]); - - // Tilt compensated Magnetic field X component: - headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; - // Tilt compensated Magnetic field Y component: - headY = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; - // Magnetic heading - heading = atan2(-headY,headX); - - // Declination correction (if supplied) - if( 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 AP_Compass_HIL::set_orientation(const Matrix3f &rotation_matrix) -{ - orientation_matrix = rotation_matrix; - if( orientation_matrix == ROTATION_NONE ) - orientation = 0; - else - orientation = 1; -} - -void AP_Compass_HIL::set_offsets(int x, int y, int z) -{ - offset[0] = x; - offset[1] = y; - offset[2] = z; -} - -void AP_Compass_HIL::set_declination(float radians) -{ - declination = radians; -} - +// Update raw magnetometer values from HIL data +// void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z) { // TODO: map floats to raw diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index 67a13c0545..ec5dfcff0b 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -1,29 +1,16 @@ #ifndef AP_Compass_HIL_H #define AP_Compass_HIL_H -#include -#include "AP_Compass_HMC5843.h" // to get #defines since we are modelling this -#include "WProgram.h" +#include "Compass.h" class AP_Compass_HIL : public Compass { public: - AP_Compass_HIL(); // Constructor - - bool init(int initialise_wire_lib = 1); - void read(); - void calculate(float roll, float pitch); - void set_orientation(const Matrix3f &rotation_matrix); - void set_offsets(int x, int y, int z); - void set_declination(float radians); - void setHIL(float Mag_X, float Mag_Y, float Mag_Z); + AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} - private: - int orientation; - Matrix3f orientation_matrix; - float calibration[3]; - int offset[3]; - float declination; + virtual bool init(); + virtual void read(); + void setHIL(float Mag_X, float Mag_Y, float Mag_Z); }; #endif diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 688a65c7cc..f32fbeb500 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -10,28 +10,7 @@ Sensor is conected to I2C port Sensor is initialized in Continuos mode (10Hz) - - Variables: - heading : magnetic heading - heading_x : magnetic heading X component - heading_y : magnetic heading Y component - mag_x : Raw X axis magnetometer data - mag_y : Raw Y axis magnetometer data - mag_z : Raw Z axis magnetometer data - last_update : the time of the last successful reading - - Methods: - init() : Initialization of I2C and sensor - read() : Read Sensor data - calculate(float roll, float pitch) : Calculate tilt adjusted heading - set_orientation(const Matrix3f &rotation_matrix) : Set orientation of compass - set_offsets(int x, int y, int z) : Set adjustments for HardIron disturbances - set_declination(float radians) : Set heading adjustment between true north and magnetic north - To do : code optimization - Mount position : UPDATED - Big capacitor pointing backward, connector forward - */ // AVR LibC Includes @@ -52,42 +31,25 @@ #define ContinuousConversion 0x00 #define SingleConversion 0x01 -// Constructors //////////////////////////////////////////////////////////////// -AP_Compass_HMC5843::AP_Compass_HMC5843() : orientation(0), declination(0.0) -{ - // mag x y z offset initialisation - - _offset.x = 0; - _offset.y = 0; - _offset.z = 0; - - // initialise orientation matrix - orientation_matrix = ROTATION_NONE; -} - // Public Methods ////////////////////////////////////////////////////////////// -bool AP_Compass_HMC5843::init(int initialise_wire_lib) +bool AP_Compass_HMC5843::init() { unsigned long currentTime = millis(); // record current time int numAttempts = 0; int success = 0; - first_call = 1; - - if( initialise_wire_lib != 0 ) - Wire.begin(); - + delay(10); - + // calibration initialisation calibration[0] = 1.0; calibration[1] = 1.0; calibration[2] = 1.0; - + while( success == 0 && numAttempts < 5 ) { // record number of attempts at initialisation numAttempts++; - + // force positiveBias (compass should return 715 for all channels) Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ConfigRegA); @@ -95,20 +57,20 @@ bool AP_Compass_HMC5843::init(int initialise_wire_lib) if (0 != Wire.endTransmission()) continue; // compass not responding on the bus delay(50); - + // set gains Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ConfigRegB); Wire.send(magGain); Wire.endTransmission(); - delay(10); + delay(10); Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ModeRegister); Wire.send(SingleConversion); Wire.endTransmission(); delay(10); - + // read values from the compass read(); delay(10); @@ -119,11 +81,11 @@ bool AP_Compass_HMC5843::init(int initialise_wire_lib) calibration[0] = fabs(715.0 / mag_x); calibration[1] = fabs(715.0 / mag_y); calibration[2] = fabs(715.0 / mag_z); - + // mark success success = 1; } - + // leave test mode Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ConfigRegA); @@ -146,20 +108,20 @@ void AP_Compass_HMC5843::read() int i = 0; byte buff[6]; Vector3f rot_mag; - - Wire.beginTransmission(COMPASS_ADDRESS); + + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(0x03); //sends address to read from Wire.endTransmission(); //end transmission - - //Wire.beginTransmission(COMPASS_ADDRESS); + + //Wire.beginTransmission(COMPASS_ADDRESS); Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device - while(Wire.available()) - { + while(Wire.available()) + { buff[i] = Wire.receive(); // receive one byte i++; } Wire.endTransmission(); //end transmission - + if (i==6) // All bytes received? { // MSB byte first, then LSB, X,Y,Z @@ -167,102 +129,13 @@ 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; + // rotate and offset the magnetometer values + // XXX this could well be done in common code... + rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z); + rot_mag = rot_mag + _offset.get(); mag_x = rot_mag.x; mag_y = rot_mag.y; mag_z = rot_mag.z; } } -void AP_Compass_HMC5843::calculate(float roll, float pitch) -{ - 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_pitch = cos(pitch); - sin_pitch = 1 - (cos_pitch * cos_pitch); - - // Tilt compensated magnetic field X component: - 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; - // magnetic heading - heading = atan2(-headY,headX); - - // Declination correction (if supplied) - if( 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 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; - if( orientation_matrix == ROTATION_NONE ) - orientation = 0; - else - orientation = 1; -} - -void AP_Compass_HMC5843::set_offsets(int x, int y, int z) -{ - _offset.x = x; - _offset.y = y; - _offset.z = z; -} - -void AP_Compass_HMC5843::set_declination(float radians) -{ - declination = radians; -} diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 7fb91f0e74..89601c7bde 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -1,26 +1,10 @@ #ifndef AP_Compass_HMC5843_H #define AP_Compass_HMC5843_H -#include "Compass.h" -#include "../AP_Math/AP_Math.h" +#include +#include -// Rotation matrices -#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) -#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) -#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) +#include "Compass.h" // orientations for DIYDrones magnetometer #define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE @@ -61,23 +45,10 @@ 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]; - 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); + AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} + virtual bool init(); + virtual void read(); }; #endif diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp new file mode 100644 index 0000000000..6090089c9c --- /dev/null +++ b/libraries/AP_Compass/Compass.cpp @@ -0,0 +1,122 @@ +#include "Compass.h" + +// Default constructor. +// Note that the Vector/Matrix constructors already implicitly zero +// their values. +// +Compass::Compass(AP_Var::Key key) : + _group(key), + _orientation_matrix(&_group, 0), + _offset(&_group, 1), + _declination(&_group, 2, 0.0), + _null_init_done(false) +{ + // Default the orientation matrix to none - will be overridden at group load time + // if an orientation has previously been saved. + _orientation_matrix.set(ROTATION_NONE); +} + +// Default init method, just returns success. +// +bool +Compass::init() +{ + return true; +} + +void +Compass::set_orientation(const Matrix3f &rotation_matrix) +{ + _orientation_matrix.set_and_save(rotation_matrix); +} + +void +Compass::set_offsets(const Vector3f &offsets) +{ + _offset.set(offsets); +} + +void +Compass::save_offsets() +{ + _offset.save(); +} + +Vector3f & +Compass::get_offsets() +{ + return _offset.get(); +} + +void +Compass::set_declination(float radians) +{ + _declination.set_and_save(radians); +} + +void +Compass::calculate(float roll, float pitch) +{ + 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_pitch = cos(pitch); + sin_pitch = 1 - (cos_pitch * cos_pitch); + + // Tilt compensated magnetic field X component: + 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; + // 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::null_offsets(const Matrix3f &dcm_matrix) +{ + // Update our estimate of the offsets in the magnetometer + Vector3f calc(0.0, 0.0, 0.0); // XXX should be safe to remove explicit init here as the default ctor should do the right thing + Matrix3f dcm_new_from_last; + float weight; + + Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z); + + if(_null_init_done) { + 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.set(_offset.get() - calc); + } + } else { + _null_init_done = true; + } + _mag_body_last = mag_body_new - calc; + _last_dcm_matrix = dcm_matrix; + +} diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 74a986b66d..1617a771dd 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -2,24 +2,120 @@ #define Compass_h #include -#include "../AP_Math/AP_Math.h" +#include +#include + +// standard rotation matrices +#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) +#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) +#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) +#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) +#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) +#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) +#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) +#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) +#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) +#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) class Compass { - public: - int mag_x; - int mag_y; - int mag_z; - float heading; - float heading_x; - float heading_y; - unsigned long last_update; - - virtual bool init(int initialise_wire_lib = 1) = 0; +public: + int mag_x; ///< magnetic field strength along the X axis + int mag_y; ///< magnetic field strength along the Y axis + int 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 + unsigned long last_update; ///< millis() time of last update + + /// Constructor + /// + /// @param key Storage key used for configuration data. + /// + Compass(AP_Var::Key key); + + /// Initialize the compass device. + /// + /// @returns True if the compass was initialized OK, false if it was not + /// found or is not functioning. + /// + virtual bool init(); + + /// Read the compass and update the mag_ variables. + /// virtual void read() = 0; - virtual void calculate(float roll, float pitch) = 0; - virtual void set_orientation(const Matrix3f &rotation_matrix) = 0; - virtual void set_offsets(int x, int y, int z) = 0; - virtual void set_declination(float radians) = 0; + + /// Calculate the tilt-compensated heading_ variables. + /// + /// @param roll The current airframe roll angle. + /// @param pitch The current airframe pitch angle. + /// + virtual void calculate(float roll, float pitch); + + /// Set the compass orientation matrix, used to correct for + /// various compass mounting positions. + /// + /// @param rotation_matrix Rotation matrix to transform magnetometer readings + /// to the body frame. + /// + virtual void set_orientation(const Matrix3f &rotation_matrix); + + /// Sets the compass offset x/y/z values. + /// + /// @param offsets Offsets to the raw mag_ values. + /// + virtual void set_offsets(const Vector3f &offsets); + + /// Saves the current compass offset x/y/z values. + /// + /// This should be invoked periodically to save the offset values maintained by + /// ::null_offsets. + /// + virtual void save_offsets(); + + /// Returns the current offset values + /// + /// @returns The current compass offsets. + /// + virtual Vector3f &get_offsets(); + + /// Program new offset values. + /// + /// XXX DEPRECATED + /// + /// @param x Offset to the raw mag_x value. + /// @param y Offset to the raw mag_y value. + /// @param z Offset to the raw mag_z value. + /// + void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); } + + /// Perform automatic offset updates using the results of the DCM matrix. + /// + /// @param dcm_matrix The DCM result matrix. + /// + void null_offsets(const Matrix3f &dcm_matrix); + + + /// Sets the local magnetic field declination. + /// + /// @param radians Local field declination. + /// + virtual void set_declination(float radians); + +protected: + AP_Var_group _group; ///< storage group holding the compass' calibration data + AP_VarS _orientation_matrix; + AP_VarS _offset; + AP_Float _declination; + + 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 }; #endif diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index b57642132b..ba74e5b236 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -3,9 +3,11 @@ Code by Jordi MuÒoz and Jose Julio. DIYDrones.com */ -#include +#include +#include #include // Compass Library #include // ArduPilot Mega Vector/Matrix math Library +#include #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi @@ -19,12 +21,13 @@ void setup() { Serial.begin(38400); Serial.println("Compass library test (HMC5843)"); + Wire.begin(); compass.init(); // Initialization - + compass.set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft compass.set_offsets(0,0,0); // set offsets to account for surrounding interference compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north - + delay(1000); timer = millis(); } @@ -32,13 +35,13 @@ void setup() void loop() { static float min[3], max[3], offset[3]; - + if((millis()- timer) > 100) { timer = millis(); compass.read(); compass.calculate(0,0); // roll = 0, pitch = 0 for this example - + // capture min if( compass.mag_x < min[0] ) min[0] = compass.mag_x; @@ -46,7 +49,7 @@ void loop() min[1] = compass.mag_y; if( compass.mag_z < min[2] ) min[2] = compass.mag_z; - + // capture max if( compass.mag_x > max[0] ) max[0] = compass.mag_x; @@ -54,12 +57,12 @@ void loop() max[1] = compass.mag_y; if( compass.mag_z > max[2] ) max[2] = compass.mag_z; - + // calculate offsets offset[0] = -(max[0]+min[0])/2; offset[1] = -(max[1]+min[1])/2; offset[2] = -(max[2]+min[2])/2; - + // display all to user Serial.print("Heading:"); Serial.print(ToDeg(compass.heading)); @@ -67,7 +70,7 @@ void loop() Serial.print(compass.mag_x); Serial.print(","); Serial.print(compass.mag_y); - Serial.print(","); + Serial.print(","); Serial.print(compass.mag_z); Serial.print(")"); diff --git a/libraries/AP_Compass/examples/AP_Compass_test/Makefile b/libraries/AP_Compass/examples/AP_Compass_test/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/AP_Compass/examples/AP_Compass_test/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk