mirror of https://github.com/ArduPilot/ardupilot
Major update including AP_Var support.
Most of the compass functionality is now abstracted in a base class, with the various sub-classes implementing just their unique functionality. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1647 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
90bae93a50
commit
1f04ecbfdb
|
@ -11,121 +11,15 @@
|
||||||
|
|
||||||
#include "AP_Compass_HIL.h"
|
#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 //////////////////////////////////////////////////////////////
|
// 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()
|
void AP_Compass_HIL::read()
|
||||||
{
|
{
|
||||||
// values set by setHIL function
|
// values set by setHIL function
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_Compass_HIL::calculate(float roll, float pitch)
|
// Update raw magnetometer values from HIL data
|
||||||
{
|
//
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
|
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
|
||||||
{
|
{
|
||||||
// TODO: map floats to raw
|
// TODO: map floats to raw
|
||||||
|
|
|
@ -1,29 +1,16 @@
|
||||||
#ifndef AP_Compass_HIL_H
|
#ifndef AP_Compass_HIL_H
|
||||||
#define AP_Compass_HIL_H
|
#define AP_Compass_HIL_H
|
||||||
|
|
||||||
#include <Compass.h>
|
#include "Compass.h"
|
||||||
#include "AP_Compass_HMC5843.h" // to get #defines since we are modelling this
|
|
||||||
#include "WProgram.h"
|
|
||||||
|
|
||||||
class AP_Compass_HIL : public Compass
|
class AP_Compass_HIL : public Compass
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_Compass_HIL(); // Constructor
|
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||||
|
|
||||||
bool init(int initialise_wire_lib = 1);
|
virtual bool init();
|
||||||
void read();
|
virtual 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);
|
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
|
||||||
|
|
||||||
private:
|
|
||||||
int orientation;
|
|
||||||
Matrix3f orientation_matrix;
|
|
||||||
float calibration[3];
|
|
||||||
int offset[3];
|
|
||||||
float declination;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -11,27 +11,6 @@
|
||||||
Sensor is conected to I2C port
|
Sensor is conected to I2C port
|
||||||
Sensor is initialized in Continuos mode (10Hz)
|
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
|
// AVR LibC Includes
|
||||||
|
@ -52,29 +31,12 @@
|
||||||
#define ContinuousConversion 0x00
|
#define ContinuousConversion 0x00
|
||||||
#define SingleConversion 0x01
|
#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 //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
bool AP_Compass_HMC5843::init(int initialise_wire_lib)
|
bool AP_Compass_HMC5843::init()
|
||||||
{
|
{
|
||||||
unsigned long currentTime = millis(); // record current time
|
unsigned long currentTime = millis(); // record current time
|
||||||
int numAttempts = 0;
|
int numAttempts = 0;
|
||||||
int success = 0;
|
int success = 0;
|
||||||
first_call = 1;
|
|
||||||
|
|
||||||
if( initialise_wire_lib != 0 )
|
|
||||||
Wire.begin();
|
|
||||||
|
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
|
@ -167,102 +129,13 @@ void AP_Compass_HMC5843::read()
|
||||||
mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y 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
|
mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
|
||||||
last_update = millis(); // record time of update
|
last_update = millis(); // record time of update
|
||||||
// rotate the magnetometer values depending upon orientation
|
// rotate and offset the magnetometer values
|
||||||
if( orientation == 0 )
|
// XXX this could well be done in common code...
|
||||||
rot_mag = Vector3f(mag_x,mag_y,mag_z);
|
rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
|
||||||
else
|
rot_mag = rot_mag + _offset.get();
|
||||||
rot_mag = orientation_matrix*Vector3f(mag_x,mag_y,mag_z);
|
|
||||||
rot_mag = rot_mag + _offset;
|
|
||||||
mag_x = rot_mag.x;
|
mag_x = rot_mag.x;
|
||||||
mag_y = rot_mag.y;
|
mag_y = rot_mag.y;
|
||||||
mag_z = rot_mag.z;
|
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;
|
|
||||||
}
|
|
||||||
|
|
|
@ -1,26 +1,10 @@
|
||||||
#ifndef AP_Compass_HMC5843_H
|
#ifndef AP_Compass_HMC5843_H
|
||||||
#define AP_Compass_HMC5843_H
|
#define AP_Compass_HMC5843_H
|
||||||
|
|
||||||
#include "Compass.h"
|
#include <AP_Common.h>
|
||||||
#include "../AP_Math/AP_Math.h"
|
#include <AP_Math.h>
|
||||||
|
|
||||||
// Rotation matrices
|
#include "Compass.h"
|
||||||
#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)
|
|
||||||
|
|
||||||
// orientations for DIYDrones magnetometer
|
// orientations for DIYDrones magnetometer
|
||||||
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
|
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
|
||||||
|
@ -61,23 +45,10 @@
|
||||||
class AP_Compass_HMC5843 : public Compass
|
class AP_Compass_HMC5843 : public Compass
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
int orientation;
|
|
||||||
Matrix3f orientation_matrix;
|
|
||||||
Matrix3f last_dcm_matrix;
|
|
||||||
Vector3f mag_body_last;
|
|
||||||
bool first_call;
|
|
||||||
float calibration[3];
|
float calibration[3];
|
||||||
Vector3f _offset;
|
|
||||||
float declination;
|
|
||||||
public:
|
public:
|
||||||
AP_Compass_HMC5843(); // Constructor
|
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||||
bool init(int initialiseWireLib = 1);
|
virtual bool init();
|
||||||
void read();
|
virtual 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
|
#endif
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
}
|
|
@ -2,24 +2,120 @@
|
||||||
#define Compass_h
|
#define Compass_h
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
#include "../AP_Math/AP_Math.h"
|
#include <AP_Common.h>
|
||||||
|
#include <AP_Math.h>
|
||||||
|
|
||||||
|
// 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
|
class Compass
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
int mag_x;
|
int mag_x; ///< magnetic field strength along the X axis
|
||||||
int mag_y;
|
int mag_y; ///< magnetic field strength along the Y axis
|
||||||
int mag_z;
|
int mag_z; ///< magnetic field strength along the Z axis
|
||||||
float heading;
|
float heading; ///< compass heading in radians
|
||||||
float heading_x;
|
float heading_x; ///< compass vector X magnitude
|
||||||
float heading_y;
|
float heading_y; ///< compass vector Y magnitude
|
||||||
unsigned long last_update;
|
unsigned long last_update; ///< millis() time of last update
|
||||||
|
|
||||||
virtual bool init(int initialise_wire_lib = 1) = 0;
|
/// 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 read() = 0;
|
||||||
virtual void calculate(float roll, float pitch) = 0;
|
|
||||||
virtual void set_orientation(const Matrix3f &rotation_matrix) = 0;
|
/// Calculate the tilt-compensated heading_ variables.
|
||||||
virtual void set_offsets(int x, int y, int z) = 0;
|
///
|
||||||
virtual void set_declination(float radians) = 0;
|
/// @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<Matrix3f> _orientation_matrix;
|
||||||
|
AP_VarS<Vector3f> _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
|
#endif
|
||||||
|
|
|
@ -3,9 +3,11 @@
|
||||||
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
|
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <Wire.h>
|
#include <FastSerial.h>
|
||||||
|
#include <AP_Common.h>
|
||||||
#include <AP_Compass.h> // Compass Library
|
#include <AP_Compass.h> // Compass Library
|
||||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||||
|
@ -19,6 +21,7 @@ void setup()
|
||||||
{
|
{
|
||||||
Serial.begin(38400);
|
Serial.begin(38400);
|
||||||
Serial.println("Compass library test (HMC5843)");
|
Serial.println("Compass library test (HMC5843)");
|
||||||
|
Wire.begin();
|
||||||
compass.init(); // Initialization
|
compass.init(); // Initialization
|
||||||
|
|
||||||
compass.set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft
|
compass.set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft
|
||||||
|
|
|
@ -0,0 +1,2 @@
|
||||||
|
BOARD = mega
|
||||||
|
include ../../../AP_Common/Arduino.mk
|
Loading…
Reference in New Issue