Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Compass: update the compass driver to use the new vector.rotate() method

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-10 13:45:35 +11:00
parent 24a9fe8827
commit 3c145ab61c
4 changed files with 18 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -71,16 +71,6 @@ bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
return true;
}
/*
the 5883L has a different orientation to the 5843. This allows us to
use a single MAG_ORIENTATION for both
*/
static void rotate_for_5883L(AP_Matrix3f *_orientation_matrix)
{
_orientation_matrix->set_and_save(_orientation_matrix->get() * Matrix3f(ROTATION_YAW_90));
}
// Read Sensor data
bool AP_Compass_HMC5843::read_raw()
{
@ -148,20 +138,11 @@ AP_Compass_HMC5843::init()
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
int old_product_id = product_id;
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
if (old_product_id != product_id) {
/* now we know the compass type we need to rotate the
* orientation matrix that we were given
*/
rotate_for_5883L(&_orientation_matrix);
}
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
@ -280,11 +261,15 @@ bool AP_Compass_HMC5843::read()
mag_z *= calibration[2];
last_update = micros(); // record time of update
// rotate and offset the magnetometer values
// XXX this could well be done in common code...
Vector3f rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
rot_mag = rot_mag + _offset.get();
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
rot_mag.rotate((enum Rotation)_orientation.get());
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
}
rot_mag += _offset.get();
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
@ -295,10 +280,7 @@ bool AP_Compass_HMC5843::read()
// set orientation
void
AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix)
AP_Compass_HMC5843::set_orientation(enum Rotation rotation)
{
_orientation_matrix.set_and_save(rotation_matrix);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rotate_for_5883L(&_orientation_matrix);
}
_orientation.set_and_save(rotation);
}

2
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -60,7 +60,7 @@ class AP_Compass_HMC5843 : public Compass
AP_Compass_HMC5843() : Compass() {}
virtual bool init(void);
virtual bool read(void);
virtual void set_orientation(const Matrix3f &rotation_matrix);
virtual void set_orientation(enum Rotation rotation);
};
#endif

11
libraries/AP_Compass/Compass.cpp
View File

@ -2,11 +2,12 @@
#include "Compass.h"
const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("ORIENT", 0, Compass, _orientation_matrix),
// index 0 was used for the old orientation matrix
AP_GROUPINFO("OFS", 1, Compass, _offset),
AP_GROUPINFO("DEC", 2, Compass, _declination),
AP_GROUPINFO("LEARN", 3, Compass, _learn), // true if learning calibration
AP_GROUPINFO("USE", 4, Compass, _use_for_yaw), // true if used for DCM yaw
AP_GROUPINFO("ORIENT", 5, Compass, _orientation),
AP_GROUPEND
};
@ -22,9 +23,7 @@ Compass::Compass(void) :
_null_enable(false),
_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);
_orientation.set(ROTATION_NONE);
}
// Default init method, just returns success.
@ -36,9 +35,9 @@ Compass::init()
}
void
Compass::set_orientation(const Matrix3f &rotation_matrix)
Compass::set_orientation(enum Rotation rotation)
{
_orientation_matrix.set_and_save(rotation_matrix);
_orientation.set_and_save(rotation);
}
void

22
libraries/AP_Compass/Compass.h
View File

@ -12,24 +12,6 @@
#define AP_COMPASS_TYPE_HMC5843 0x02
#define AP_COMPASS_TYPE_HMC5883L 0x03
// 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:
@ -79,7 +61,7 @@ public:
/// @param rotation_matrix Rotation matrix to transform magnetometer readings
/// to the body frame.
///
virtual void set_orientation(const Matrix3f &rotation_matrix);
virtual void set_orientation(enum Rotation rotation);
/// Sets the compass offset x/y/z values.
///
@ -137,7 +119,7 @@ public:
static const struct AP_Param::GroupInfo var_info[];
protected:
AP_Matrix3f _orientation_matrix;
AP_Int8 _orientation;
AP_Vector3f _offset;
AP_Float _declination;
AP_Int8 _learn; ///<enable calibration learning