ardupilot/libraries/AP_Compass/Compass.cpp

257 lines
6.6 KiB
C++
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Compass.h"
2012-02-11 07:53:30 -04:00
const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix
2012-02-12 18:55:13 -04:00
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_GROUPEND
2012-02-11 07:53:30 -04:00
};
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2012-02-11 07:53:30 -04:00
Compass::Compass(void) :
2012-02-29 09:57:35 -04:00
product_id(AP_COMPASS_TYPE_UNKNOWN),
2012-02-11 07:53:30 -04:00
_declination (0.0),
_learn(1),
_use_for_yaw(1),
_null_enable(false),
_null_init_done(false),
_orientation(ROTATION_NONE)
{
}
// Default init method, just returns success.
//
bool
Compass::init()
{
return true;
}
void
Compass::set_orientation(enum Rotation rotation)
{
_orientation = rotation;
}
void
Compass::set_offsets(const Vector3f &offsets)
{
_offset.set(offsets);
}
void
Compass::save_offsets()
{
_offset.save();
}
Vector3f &
Compass::get_offsets()
{
2012-02-11 07:53:30 -04:00
return _offset;
}
bool
Compass::set_initial_location(long latitude, long longitude, bool force)
{
// If the user has choosen to use auto-declination regardless of the planner value
// OR
// If the declination failed to load from the EEPROM (ie. not set by user)
if(force || !_declination.load())
{
// Set the declination based on the lat/lng from GPS
_declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
// Reset null offsets
null_offsets_disable();
null_offsets_enable();
return true;
}
return false;
}
void
Compass::set_declination(float radians)
{
_declination.set_and_save(radians);
}
float
Compass::get_declination()
{
return _declination.get();
}
void
Compass::calculate(float roll, float pitch)
{
// Note - This function implementation is deprecated
// The alternate implementation of this function using the dcm matrix is preferred
float headX;
float headY;
float cos_roll;
float sin_roll;
float cos_pitch;
float sin_pitch;
cos_roll = cos(roll);
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(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::calculate(const Matrix3f &dcm_matrix)
{
float headX;
float headY;
float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
// sin(pitch) = - dcm_matrix(3,1)
// cos(pitch)*sin(roll) = - dcm_matrix(3,2)
// cos(pitch)*cos(roll) = - dcm_matrix(3,3)
if (cos_pitch == 0.0) {
// we are pointing straight up or down so don't update our
// heading using the compass. Wait for the next iteration when
// we hopefully will have valid values again.
return;
}
// Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch;
// Tilt compensated magnetic field Y component:
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
heading = constrain(atan2(-headY,headX), -3.15, 3.15);
// 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);
#if 0
if (isnan(heading_x) || isnan(heading_y)) {
Serial.printf("COMPASS: c.x %f c.y %f c.z %f cos_pitch %f mag_x %d mag_y %d mag_z %d headX %f headY %f heading %f heading_x %f heading_y %f\n",
dcm_matrix.c.x,
dcm_matrix.c.y,
dcm_matrix.c.x,
cos_pitch,
(int)mag_x, (int)mag_y, (int)mag_z,
headX, headY,
heading,
heading_x, heading_y);
}
#endif
}
/*
this offset nulling algorithm is based on a paper from
Bill Premerlani
http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
*/
void
Compass::null_offsets(void)
{
if (_null_enable == false || _learn == 0) {
// auto-calibration is disabled
return;
}
Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
// this gain is set so we converge on the offsets in about 5
// minutes with a 10Hz compass
const float gain = 0.5;
const float max_change = 2.0;
if (!_null_init_done) {
// first time through
_null_init_done = true;
_mag_body_last = mag_body_new;
return;
}
Vector3f delta, diff;
float diff_length, delta_length;
diff = mag_body_new - _mag_body_last;
diff_length = diff.length();
if (diff_length == 0.0) {
// the mag vector hasn't changed - we don't get any
// information from this
return;
}
// equation 6 of Bills paper
delta = diff * (mag_body_new.length() - _mag_body_last.length()) / diff_length;
// limit the change from any one reading. This is to prevent
// single crazy readings from throwing off the offsets for a long
// time
delta_length = delta.length();
if (delta_length > max_change) {
delta *= max_change / delta_length;
}
// set the new offsets
_offset.set(_offset.get() - (delta * gain));
// remember the last mag vector
_mag_body_last = mag_body_new;
}
Bug fix for compass. This is a fix for an interesting bug when a DCM matrix reset was added to the ground start. This bug only showed up if (A) a ground start were performed after an air start or due to use of the "Calibrate Gryo" action, (B) if the current orientation were sufficiently different from 0/0/0, and (C.) if the particular magnetometer had sufficiently large offsets. Why did resetting the DCM matrix to 0/0/0 pitch/roll/yaw at ground start cause a bug? The magnetometer offset nulling determines the proper offsets for the magnetometer by comparing the observed change in the magnetic field vector with the expected change due to rotation as calculated from the rotation in the DCM matrix. This comparison is made at 10Hz, and then filtered with a weight based on the amount of rotation to estimate the offsets. Normally it would take considerable time at normal in-flight rotation rates for the offset estimate to converge. If a DCM matrix reset occurs when the offset nulling algorithm is up and running, the algorithm sees the DCM reset as a instantaneous rotation, however the magnetic field vector did not change at all. Under certain conditions the algorithm would interpret this as indicating that the offset(s) should be very large. Since the "rotation" could also have been large the filter weighting would be large and it was possible for a large erroneous estimate of the offset(s) to be made based on this single (bad) data point. To fix this bug methods were added to the compass object to start and stop the offset nulling algorithm. Further, when the algorithm is started, it is set up to get fresh samples. The DCM matrix reset method now calls these new methods to stop the offset nulling before resetting the matrix, and resume after the matrix has been reset.
2012-01-12 17:43:39 -04:00
void
Compass::null_offsets_enable(void)
{
_null_init_done = false;
_null_enable = true;
}
void
Compass::null_offsets_disable(void)
{
_null_init_done = false;
_null_enable = false;
2012-02-11 07:53:30 -04:00
}