diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index a23523614d..a8554da41b 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -1,29 +1,29 @@ -/* - AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer - Code by James Goppert. DIYDrones.com - - This library is free software; you can redistribute it and / or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. -*/ - - -#include "AP_Compass_HIL.h" - -// Public Methods ////////////////////////////////////////////////////////////// - -void AP_Compass_HIL::read() -{ - // values set by setHIL function -} - -// 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 - mag_x = _mag_x; - mag_y = _mag_y; - mag_z = _mag_z; -} +/* + AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer + Code by James Goppert. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ + + +#include "AP_Compass_HIL.h" + +// Public Methods ////////////////////////////////////////////////////////////// + +void AP_Compass_HIL::read() +{ + // values set by setHIL function +} + +// 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 + mag_x = _mag_x; + mag_y = _mag_y; + mag_z = _mag_z; +} diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index ec5dfcff0b..b597d31f54 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -1,16 +1,16 @@ -#ifndef AP_Compass_HIL_H -#define AP_Compass_HIL_H - -#include "Compass.h" - -class AP_Compass_HIL : public Compass -{ - public: - AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} - - virtual bool init(); - virtual void read(); - void setHIL(float Mag_X, float Mag_Y, float Mag_Z); -}; - -#endif +#ifndef AP_Compass_HIL_H +#define AP_Compass_HIL_H + +#include "Compass.h" + +class AP_Compass_HIL : public Compass +{ + public: + AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} + + 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 f32fbeb500..4b78cfec96 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -1,141 +1,141 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Sensor is conected to I2C port - Sensor is initialized in Continuos mode (10Hz) - -*/ - -// AVR LibC Includes -#include -#include "WConstants.h" - -#include -#include "AP_Compass_HMC5843.h" - -#define COMPASS_ADDRESS 0x1E -#define ConfigRegA 0x00 -#define ConfigRegB 0x01 -#define magGain 0x20 -#define PositiveBiasConfig 0x11 -#define NegativeBiasConfig 0x12 -#define NormalOperation 0x10 -#define ModeRegister 0x02 -#define ContinuousConversion 0x00 -#define SingleConversion 0x01 - -// Public Methods ////////////////////////////////////////////////////////////// -bool AP_Compass_HMC5843::init() -{ - unsigned long currentTime = millis(); // record current time - int numAttempts = 0; - int success = 0; - - 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); - Wire.send(PositiveBiasConfig); - 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); - - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(ModeRegister); - Wire.send(SingleConversion); - Wire.endTransmission(); - delay(10); - - // 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; - } - - // leave test mode - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(ConfigRegA); - Wire.send(NormalOperation); - Wire.endTransmission(); - delay(50); - - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(ModeRegister); - Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) - Wire.endTransmission(); // End transmission - delay(50); - } - return(success); -} - -// Read Sensor data -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 - Wire.endTransmission(); //end transmission - - //Wire.beginTransmission(COMPASS_ADDRESS); - Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device - 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 - mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis - 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 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; - } -} - +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sensor is conected to I2C port + Sensor is initialized in Continuos mode (10Hz) + +*/ + +// AVR LibC Includes +#include +#include "WConstants.h" + +#include +#include "AP_Compass_HMC5843.h" + +#define COMPASS_ADDRESS 0x1E +#define ConfigRegA 0x00 +#define ConfigRegB 0x01 +#define magGain 0x20 +#define PositiveBiasConfig 0x11 +#define NegativeBiasConfig 0x12 +#define NormalOperation 0x10 +#define ModeRegister 0x02 +#define ContinuousConversion 0x00 +#define SingleConversion 0x01 + +// Public Methods ////////////////////////////////////////////////////////////// +bool AP_Compass_HMC5843::init() +{ + unsigned long currentTime = millis(); // record current time + int numAttempts = 0; + int success = 0; + + 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); + Wire.send(PositiveBiasConfig); + 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); + + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(ModeRegister); + Wire.send(SingleConversion); + Wire.endTransmission(); + delay(10); + + // 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; + } + + // leave test mode + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(ConfigRegA); + Wire.send(NormalOperation); + Wire.endTransmission(); + delay(50); + + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(ModeRegister); + Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) + Wire.endTransmission(); // End transmission + delay(50); + } + return(success); +} + +// Read Sensor data +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 + Wire.endTransmission(); //end transmission + + //Wire.beginTransmission(COMPASS_ADDRESS); + Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device + 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 + mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis + 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 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; + } +} + diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 89601c7bde..c0b6e894d7 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -1,54 +1,54 @@ -#ifndef AP_Compass_HMC5843_H -#define AP_Compass_HMC5843_H - -#include -#include - -#include "Compass.h" - -// orientations for DIYDrones magnetometer -#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE -#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45 -#define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90 -#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135 -#define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180 -#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225 -#define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270 -#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270 -#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315 - -// orientations for Sparkfun magnetometer -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270 -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315 -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45 -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90 -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135 -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180 -#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180 -#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45 - -class AP_Compass_HMC5843 : public Compass -{ - private: - float calibration[3]; - public: - AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} - virtual bool init(); - virtual void read(); -}; -#endif +#ifndef AP_Compass_HMC5843_H +#define AP_Compass_HMC5843_H + +#include +#include + +#include "Compass.h" + +// orientations for DIYDrones magnetometer +#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE +#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45 +#define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90 +#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135 +#define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180 +#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225 +#define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270 +#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270 +#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315 + +// orientations for Sparkfun magnetometer +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270 +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315 +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45 +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90 +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135 +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180 +#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180 +#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45 + +class AP_Compass_HMC5843 : public Compass +{ + private: + float calibration[3]; + public: + 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.h b/libraries/AP_Compass/Compass.h index 1617a771dd..f10d902447 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -1,121 +1,121 @@ -#ifndef Compass_h -#define Compass_h - -#include -#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; ///< 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; - - /// 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 +#ifndef Compass_h +#define Compass_h + +#include +#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; ///< 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; + + /// 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