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; 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 // Read Sensor data
bool AP_Compass_HMC5843::read_raw() bool AP_Compass_HMC5843::read_raw()
{ {
@ -148,20 +138,11 @@ AP_Compass_HMC5843::init()
} }
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config // a 5883L supports the sample averaging config
int old_product_id = product_id;
product_id = AP_COMPASS_TYPE_HMC5883L; product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60; calibration_gain = 0x60;
expected_x = 766; expected_x = 766;
expected_yz = 713; expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain 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)) { } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843; product_id = AP_COMPASS_TYPE_HMC5843;
} else { } else {
@ -280,11 +261,15 @@ bool AP_Compass_HMC5843::read()
mag_z *= calibration[2]; mag_z *= calibration[2];
last_update = micros(); // record time of update 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); // rotate to the desired orientation
rot_mag = rot_mag + _offset.get(); 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_x = rot_mag.x;
mag_y = rot_mag.y; mag_y = rot_mag.y;
mag_z = rot_mag.z; mag_z = rot_mag.z;
@ -295,10 +280,7 @@ bool AP_Compass_HMC5843::read()
// set orientation // set orientation
void 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); _orientation.set_and_save(rotation);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rotate_for_5883L(&_orientation_matrix);
}
} }

2
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

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

11
libraries/AP_Compass/Compass.cpp
View File

@ -2,11 +2,12 @@
#include "Compass.h" #include "Compass.h"
const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { 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("OFS", 1, Compass, _offset),
AP_GROUPINFO("DEC", 2, Compass, _declination), AP_GROUPINFO("DEC", 2, Compass, _declination),
AP_GROUPINFO("LEARN", 3, Compass, _learn), // true if learning calibration 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("USE", 4, Compass, _use_for_yaw), // true if used for DCM yaw
AP_GROUPINFO("ORIENT", 5, Compass, _orientation),
AP_GROUPEND AP_GROUPEND
}; };
@ -22,9 +23,7 @@ Compass::Compass(void) :
_null_enable(false), _null_enable(false),
_null_init_done(false) _null_init_done(false)
{ {
// Default the orientation matrix to none - will be overridden at group load time _orientation.set(ROTATION_NONE);
// if an orientation has previously been saved.
_orientation_matrix.set(ROTATION_NONE);
} }
// Default init method, just returns success. // Default init method, just returns success.
@ -36,9 +35,9 @@ Compass::init()
} }
void 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 void

22
libraries/AP_Compass/Compass.h
View File

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