mirror of https://github.com/ArduPilot/ardupilot
123 lines
3.2 KiB
C++
123 lines
3.2 KiB
C++
#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;
|
|
|
|
}
|