2011-12-28 05:31:36 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2012-10-11 17:48:39 -03:00
# include <AP_Progmem.h>
2011-02-14 00:25:20 -04:00
# include "Compass.h"
2012-02-11 07:53:30 -04:00
const AP_Param : : GroupInfo Compass : : var_info [ ] PROGMEM = {
2012-03-09 22:45:35 -04:00
// index 0 was used for the old orientation matrix
2013-01-02 03:07:33 -04:00
// @Param: OFS_X
// @DisplayName: Compass offsets on the X axis
// @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
// @Param: OFS_Y
// @DisplayName: Compass offsets on the Y axis
// @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
// @Param: OFS_Z
// @DisplayName: Compass offsets on the Z axis
// @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " OFS " , 1 , Compass , _offset , 0 ) ,
2013-01-02 03:07:33 -04:00
// @Param: DEC
// @DisplayName: Compass declination
// @Description: An angle to compensate between the true north and magnetic north
// @Range: -3.142 3.142
// @Units: Radians
// @Increment: 0.01
// @User: Standard
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " DEC " , 2 , Compass , _declination , 0 ) ,
2013-01-02 03:07:33 -04:00
// @Param: LEARN
// @DisplayName: Learn compass offsets automatically
// @Description: Enable or disable the automatic learning of compass offsets
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " LEARN " , 3 , Compass , _learn , 1 ) , // true if learning calibration
2013-01-02 03:07:33 -04:00
// @Param: USE
// @DisplayName: Use compass for yaw
// @Description: Enable or disable the use of the compass (instead of the GPS) for determining heading
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " USE " , 4 , Compass , _use_for_yaw , 1 ) , // true if used for DCM yaw
2013-01-02 03:07:33 -04:00
2012-12-19 22:16:42 -04:00
# if !defined( __AVR_ATmega1280__ )
2013-01-02 03:07:33 -04:00
// @Param: AUTODEC
// @DisplayName: Auto Declination
// @Description: Enable or disable the automatic calculation of the declination based on gps location
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " AUTODEC " , 5 , Compass , _auto_declination , 1 ) ,
2013-03-01 10:59:53 -04:00
# endif
2013-02-27 03:57:04 -04:00
2013-03-03 10:02:12 -04:00
// @Param: MOTCT
// @DisplayName: Motor interference compensation type
2013-05-11 02:50:36 -03:00
// @Description: Set motor interference compensation type to disabled, throttle or current. Do not change manually.
2013-03-03 10:02:12 -04:00
// @Values: 0:Disabled,1:Use Throttle,2:Use Current
// @Increment: 1
AP_GROUPINFO ( " MOTCT " , 6 , Compass , _motor_comp_type , AP_COMPASS_MOT_COMP_DISABLED ) ,
2013-02-27 03:57:04 -04:00
// @Param: MOT_X
// @DisplayName: Motor interference compensation for body frame X axis
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference
2013-03-01 10:59:53 -04:00
// @Range: -1000 1000
2013-05-11 02:50:36 -03:00
// @Units: Offset per Amp or at Full Throttle
2013-02-27 03:57:04 -04:00
// @Increment: 1
// @Param: MOT_Y
// @DisplayName: Motor interference compensation for body frame Y axis
// @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference
2013-03-01 10:59:53 -04:00
// @Range: -1000 1000
2013-05-11 02:50:36 -03:00
// @Units: Offset per Amp or at Full Throttle
2013-02-27 03:57:04 -04:00
// @Increment: 1
// @Param: MOT_Z
// @DisplayName: Motor interference compensation for body frame Z axis
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference
2013-03-01 10:59:53 -04:00
// @Range: -1000 1000
2013-05-11 02:50:36 -03:00
// @Units: Offset per Amp or at Full Throttle
2013-02-27 03:57:04 -04:00
// @Increment: 1
2013-03-03 10:02:12 -04:00
AP_GROUPINFO ( " MOT " , 7 , Compass , _motor_compensation , 0 ) ,
2013-02-27 03:57:04 -04:00
2013-05-01 23:27:35 -03:00
// @Param: ORIENT
// @DisplayName: Compass orientation
2013-06-26 23:12:49 -03:00
// @Description: The orientation of the compass relative to the autopilot board. This will default to the right value for each board type, but can be changed if you have an external compass. See the documentation for your external compass for the right value. The correct orientation should give the X axis forward, the Y axis to the right and the Z axis down. So if your aircraft is pointing west it should show a positive value for the Y axis, and a value close to zero for the X axis. NOTE: This orientation is combined with any AHRS_ORIENTATION setting.
2013-07-07 23:07:18 -03:00
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270
2013-05-01 23:27:35 -03:00
AP_GROUPINFO ( " ORIENT " , 8 , Compass , _orientation , ROTATION_NONE ) ,
2012-02-12 03:23:19 -04:00
AP_GROUPEND
2012-02-11 07:53:30 -04:00
} ;
2011-02-14 00:25:20 -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-08-17 03:19:22 -03:00
product_id ( AP_COMPASS_TYPE_UNKNOWN ) ,
2012-04-09 05:30:53 -03:00
_null_init_done ( false )
2011-02-14 00:25:20 -04:00
{
2012-12-12 17:43:51 -04:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2011-02-14 00:25:20 -04:00
}
// Default init method, just returns success.
//
bool
Compass : : init ( )
{
return true ;
}
void
Compass : : set_offsets ( const Vector3f & offsets )
{
_offset . set ( offsets ) ;
}
void
Compass : : save_offsets ( )
{
_offset . save ( ) ;
}
2013-04-21 09:24:26 -03:00
const Vector3f &
Compass : : get_offsets ( ) const
2011-02-14 00:25:20 -04:00
{
2012-02-11 07:53:30 -04:00
return _offset ;
2011-02-14 00:25:20 -04:00
}
2013-02-27 03:57:04 -04:00
void
Compass : : set_motor_compensation ( const Vector3f & motor_comp_factor )
{
_motor_compensation . set ( motor_comp_factor ) ;
}
void
Compass : : save_motor_compensation ( )
{
2013-03-03 10:02:12 -04:00
_motor_comp_type . save ( ) ;
2013-02-27 03:57:04 -04:00
_motor_compensation . save ( ) ;
}
2012-03-30 00:18:06 -03:00
void
2012-08-18 08:41:38 -03:00
Compass : : set_initial_location ( int32_t latitude , int32_t longitude )
2012-03-10 19:19:04 -04:00
{
2012-03-30 00:18:06 -03:00
// if automatic declination is configured, then compute
// the declination based on the initial GPS fix
2012-12-19 22:16:42 -04:00
# if !defined( __AVR_ATmega1280__ )
2012-08-17 03:19:22 -03:00
if ( _auto_declination ) {
// Set the declination based on the lat/lng from GPS
2012-10-11 17:48:39 -03:00
_declination . set ( radians (
AP_Declination : : get_declination (
( float ) latitude / 10000000 ,
( float ) longitude / 10000000 ) ) ) ;
2012-08-17 03:19:22 -03:00
}
2012-12-19 22:16:42 -04:00
# endif
2012-03-10 19:19:04 -04:00
}
2011-02-14 00:25:20 -04:00
void
2013-04-15 09:50:44 -03:00
Compass : : set_declination ( float radians , bool save_to_eeprom )
2011-02-14 00:25:20 -04:00
{
2013-04-15 09:50:44 -03:00
if ( save_to_eeprom ) {
_declination . set_and_save ( radians ) ;
} else {
_declination . set ( radians ) ;
}
2011-02-14 00:25:20 -04:00
}
2011-02-18 23:57:53 -04:00
float
2013-05-08 20:22:00 -03:00
Compass : : get_declination ( ) const
2011-02-18 23:57:53 -04:00
{
2012-08-17 03:19:22 -03:00
return _declination . get ( ) ;
2011-02-18 23:57:53 -04:00
}
2013-08-18 08:08:34 -03:00
/*
calculate a compass heading given the attitude from DCM and the mag vector
*/
2012-06-20 06:30:30 -03:00
float
2013-05-08 20:22:00 -03:00
Compass : : calculate_heading ( const Matrix3f & dcm_matrix ) const
2011-02-14 00:25:20 -04:00
{
2013-08-18 08:08:34 -03:00
float cos_pitch_sq = 1.0f - ( dcm_matrix . c . x * dcm_matrix . c . x ) ;
2011-05-08 15:15:29 -03:00
// Tilt compensated magnetic field Y component:
2013-05-08 20:19:01 -03:00
float headY = mag_y * dcm_matrix . c . z - mag_z * dcm_matrix . c . y ;
2012-02-23 19:42:03 -04:00
2011-02-14 00:25:20 -04:00
// Tilt compensated magnetic field X component:
2013-08-18 08:08:34 -03:00
float headX = mag_x * cos_pitch_sq - dcm_matrix . c . x * ( mag_y * dcm_matrix . c . y + mag_z * dcm_matrix . c . z ) ;
2013-05-08 20:19:01 -03:00
2011-02-14 00:25:20 -04:00
// magnetic heading
2011-07-08 00:56:04 -03:00
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
2013-05-08 20:19:01 -03:00
float heading = constrain_float ( atan2f ( - headY , headX ) , - 3.15f , 3.15f ) ;
2011-02-14 00:25:20 -04:00
// Declination correction (if supplied)
2013-01-10 14:42:24 -04:00
if ( fabsf ( _declination ) > 0.0f )
2011-02-14 00:25:20 -04:00
{
heading = heading + _declination ;
2013-01-10 14:42:24 -04:00
if ( heading > PI ) // Angle normalization (-180 deg, 180 deg)
heading - = ( 2.0f * PI ) ;
else if ( heading < - PI )
heading + = ( 2.0f * PI ) ;
2011-02-14 00:25:20 -04:00
}
2012-06-20 06:30:30 -03:00
return heading ;
2011-02-14 00:25:20 -04:00
}
2012-03-27 01:16:00 -03:00
/*
2012-08-17 03:19:22 -03:00
* this offset nulling algorithm is inspired by this paper from Bill Premerlani
*
* http : //gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
*
* The base algorithm works well , but is quite sensitive to
* noise . After long discussions with Bill , the following changes were
* made :
*
* 1 ) we keep a history buffer that effectively divides the mag
* vectors into a set of N streams . The algorithm is run on the
* streams separately
*
* 2 ) within each stream we only calculate a change when the mag
* vector has changed by a significant amount .
*
* This gives us the property that we learn quickly if there is no
* noise , but still learn correctly ( and slowly ) in the face of lots of
* noise .
2012-03-27 01:16:00 -03:00
*/
2011-02-14 00:25:20 -04:00
void
2012-03-27 01:16:00 -03:00
Compass : : null_offsets ( void )
2011-02-14 00:25:20 -04:00
{
2012-06-20 06:30:30 -03:00
if ( _learn = = 0 ) {
2012-02-24 23:11:07 -04:00
// auto-calibration is disabled
return ;
}
2012-03-27 01:16:00 -03:00
// this gain is set so we converge on the offsets in about 5
// minutes with a 10Hz compass
2012-03-28 06:46:11 -03:00
const float gain = 0.01 ;
const float max_change = 10.0 ;
const float min_diff = 50.0 ;
Vector3f ofs ;
ofs = _offset . get ( ) ;
2012-03-27 01:16:00 -03:00
if ( ! _null_init_done ) {
// first time through
2011-02-14 00:25:20 -04:00
_null_init_done = true ;
2012-03-28 06:46:11 -03:00
for ( uint8_t i = 0 ; i < _mag_history_size ; i + + ) {
// fill the history buffer with the current mag vector,
// with the offset removed
2013-01-10 14:42:24 -04:00
_mag_history [ i ] = Vector3i ( ( mag_x + 0.5f ) - ofs . x , ( mag_y + 0.5f ) - ofs . y , ( mag_z + 0.5f ) - ofs . z ) ;
2012-03-28 06:46:11 -03:00
}
_mag_history_index = 0 ;
2012-03-27 01:16:00 -03:00
return ;
2011-02-14 00:25:20 -04:00
}
2012-03-27 01:16:00 -03:00
2012-03-28 06:46:11 -03:00
Vector3f b1 , b2 , diff ;
float length ;
// get a past element
b1 = Vector3f ( _mag_history [ _mag_history_index ] . x ,
_mag_history [ _mag_history_index ] . y ,
_mag_history [ _mag_history_index ] . z ) ;
// the history buffer doesn't have the offsets
b1 + = ofs ;
// get the current vector
b2 = Vector3f ( mag_x , mag_y , mag_z ) ;
// calculate the delta for this sample
diff = b2 - b1 ;
length = diff . length ( ) ;
if ( length < min_diff ) {
// the mag vector hasn't changed enough - we don't get
2012-03-28 08:40:32 -03:00
// enough information from this vector to use it.
// Note that we don't put the current vector into the mag
// history here. We want to wait for a larger rotation to
// build up before calculating an offset change, as accuracy
// of the offset change is highly dependent on the size of the
// rotation.
2012-03-28 06:46:11 -03:00
_mag_history_index = ( _mag_history_index + 1 ) % _mag_history_size ;
2012-03-27 01:16:00 -03:00
return ;
}
2012-03-28 06:46:11 -03:00
// put the vector in the history
2013-01-10 14:42:24 -04:00
_mag_history [ _mag_history_index ] = Vector3i ( ( mag_x + 0.5f ) - ofs . x , ( mag_y + 0.5f ) - ofs . y , ( mag_z + 0.5f ) - ofs . z ) ;
2012-03-28 06:46:11 -03:00
_mag_history_index = ( _mag_history_index + 1 ) % _mag_history_size ;
2012-03-27 01:16:00 -03:00
// equation 6 of Bills paper
2012-03-28 06:46:11 -03:00
diff = diff * ( gain * ( b2 . length ( ) - b1 . length ( ) ) / length ) ;
2012-03-27 01:16:00 -03:00
// limit the change from any one reading. This is to prevent
// single crazy readings from throwing off the offsets for a long
// time
2012-03-28 06:46:11 -03:00
length = diff . length ( ) ;
if ( length > max_change ) {
diff * = max_change / length ;
2012-03-27 01:16:00 -03:00
}
// set the new offsets
2012-03-28 06:46:11 -03:00
_offset . set ( _offset . get ( ) - diff ) ;
2011-02-14 00:25:20 -04:00
}