2011-12-28 05:31:36 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2011-02-14 00:27:07 -04:00
# ifndef Compass_h
# define Compass_h
# include <inttypes.h>
2012-08-20 20:22:44 -03:00
# include <AP_Common.h>
# include <AP_Param.h>
# include <AP_Math.h>
# include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
2011-02-14 00:27:07 -04:00
2011-06-28 13:30:42 -03:00
// compass product id
# define AP_COMPASS_TYPE_UNKNOWN 0x00
# define AP_COMPASS_TYPE_HIL 0x01
# define AP_COMPASS_TYPE_HMC5843 0x02
# define AP_COMPASS_TYPE_HMC5883L 0x03
2013-01-04 01:10:51 -04:00
# define AP_COMPASS_TYPE_PX4 0x04
2014-03-31 14:47:42 -03:00
# define AP_COMPASS_TYPE_VRBRAIN 0x05
2011-06-28 13:30:42 -03:00
2013-03-03 10:02:12 -04:00
// motor compensation types (for use with motor_comp_enabled)
# define AP_COMPASS_MOT_COMP_DISABLED 0x00
# define AP_COMPASS_MOT_COMP_THROTTLE 0x01
# define AP_COMPASS_MOT_COMP_CURRENT 0x02
2013-05-01 23:27:35 -03:00
// setup default mag orientation for each board type
# if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define MAG_BOARD_ORIENTATION ROTATION_ROLL_180
# elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define MAG_BOARD_ORIENTATION ROTATION_NONE
2013-09-23 00:20:15 -03:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
# define MAG_BOARD_ORIENTATION ROTATION_NONE
2013-05-01 23:27:35 -03:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define MAG_BOARD_ORIENTATION ROTATION_NONE
# elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
# define MAG_BOARD_ORIENTATION ROTATION_NONE
2013-09-22 03:01:40 -03:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
2014-08-18 02:58:05 -03:00
# define MAG_BOARD_ORIENTATION ROTATION_NONE
2014-03-31 14:47:42 -03:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define MAG_BOARD_ORIENTATION ROTATION_NONE
2013-05-01 23:27:35 -03:00
# else
# error "You must define a default compass orientation for this board"
# endif
2013-12-09 01:05:57 -04:00
/**
maximum number of compass instances available on this platform . If more
than 1 then redundent sensors may be available
*/
# if CONFIG_HAL_BOARD == HAL_BOARD_PX4
2014-07-03 23:07:47 -03:00
# define COMPASS_MAX_INSTANCES 3
2014-05-30 17:58:34 -03:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define COMPASS_MAX_INSTANCES 2
2013-12-09 01:05:57 -04:00
# else
# define COMPASS_MAX_INSTANCES 1
# endif
2011-02-14 00:27:07 -04:00
class Compass
{
public :
2012-08-21 23:19:51 -03:00
int16_t product_id ; /// product id
uint32_t last_update ; ///< micros() time of last update
/// Constructor
///
Compass ( ) ;
/// Initialize the compass device.
///
/// @returns True if the compass was initialized OK, false if it was not
/// found or is not functioning.
///
virtual bool init ( ) ;
/// Read the compass and update the mag_ variables.
///
virtual bool read ( void ) = 0 ;
2013-12-08 23:09:34 -04:00
2012-08-26 00:42:00 -03:00
/// use spare CPU cycles to accumulate values from the compass if
/// possible
virtual void accumulate ( void ) = 0 ;
2012-08-21 23:19:51 -03:00
/// Calculate the tilt-compensated heading_ variables.
///
/// @param dcm_matrix The current orientation rotation matrix
2012-06-20 06:30:30 -03:00
///
/// @returns heading in radians
2012-08-21 23:19:51 -03:00
///
2013-05-08 20:22:00 -03:00
float calculate_heading ( const Matrix3f & dcm_matrix ) const ;
2012-08-21 23:19:51 -03:00
2014-10-14 19:16:31 -03:00
/// Sets offset x/y/z values.
///
/// @param i compass instance
/// @param offsets Offsets to the raw mag_ values.
///
void set_offsets ( uint8_t i , const Vector3f & offsets ) ;
2014-07-09 05:09:02 -03:00
/// Sets and saves the compass offset x/y/z values.
2012-08-21 23:19:51 -03:00
///
2014-07-09 05:09:02 -03:00
/// @param i compass instance
2012-08-21 23:19:51 -03:00
/// @param offsets Offsets to the raw mag_ values.
///
2014-07-09 05:09:02 -03:00
void set_and_save_offsets ( uint8_t i , const Vector3f & offsets ) ;
2012-08-21 23:19:51 -03:00
2014-07-09 05:07:30 -03:00
/// Saves the current offset x/y/z values for one or all compasses
///
/// @param i compass instance
2012-08-21 23:19:51 -03:00
///
/// This should be invoked periodically to save the offset values maintained by
2014-02-15 22:21:06 -04:00
/// ::learn_offsets.
2012-08-21 23:19:51 -03:00
///
2014-07-09 05:07:30 -03:00
void save_offsets ( uint8_t i ) ;
void save_offsets ( void ) ;
2012-08-21 23:19:51 -03:00
2013-12-09 01:05:57 -04:00
// return the number of compass instances
virtual uint8_t get_count ( void ) const { return 1 ; }
2013-12-08 23:09:34 -04:00
/// Return the current field as a Vector3f
2013-12-09 01:05:57 -04:00
const Vector3f & get_field ( uint8_t i ) const { return _field [ i ] ; }
2014-07-22 04:30:33 -03:00
const Vector3f & get_field ( void ) const { return get_field ( get_primary ( ) ) ; }
2013-12-08 23:09:34 -04:00
2013-12-09 02:46:41 -04:00
/// Return the health of a compass
bool healthy ( uint8_t i ) const { return _healthy [ i ] ; }
2014-07-22 04:30:33 -03:00
bool healthy ( void ) const { return healthy ( get_primary ( ) ) ; }
2013-12-09 02:46:41 -04:00
2013-12-08 23:09:34 -04:00
/// set the current field as a Vector3f
2013-12-09 01:05:57 -04:00
void set_field ( const Vector3f & field ) { _field [ 0 ] = field ; }
2013-12-08 23:09:34 -04:00
2012-08-21 23:19:51 -03:00
/// Returns the current offset values
///
/// @returns The current compass offsets.
///
2013-12-09 02:33:54 -04:00
const Vector3f & get_offsets ( uint8_t i ) const { return _offset [ i ] ; }
2014-07-22 04:30:33 -03:00
const Vector3f & get_offsets ( void ) const { return get_offsets ( get_primary ( ) ) ; }
2012-08-21 23:19:51 -03:00
/// Sets the initial location used to get declination
///
/// @param latitude GPS Latitude.
/// @param longitude GPS Longitude.
///
void set_initial_location ( int32_t latitude , int32_t longitude ) ;
/// Program new offset values.
///
2014-07-09 05:09:02 -03:00
/// @param i compass instance
2012-08-21 23:19:51 -03:00
/// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value.
///
2014-07-09 05:09:02 -03:00
void set_and_save_offsets ( uint8_t i , int x , int y , int z ) {
set_and_save_offsets ( i , Vector3f ( x , y , z ) ) ;
2012-08-21 23:19:51 -03:00
}
2014-07-09 05:09:32 -03:00
// learn offsets accessor
bool learn_offsets_enabled ( ) const { return _learn ; }
2012-08-21 23:19:51 -03:00
/// Perform automatic offset updates
///
2014-02-15 22:21:06 -04:00
void learn_offsets ( void ) ;
2011-02-14 00:27:07 -04:00
2012-02-24 23:11:07 -04:00
/// return true if the compass should be used for yaw calculations
2014-09-18 09:56:13 -03:00
bool use_for_yaw ( uint8_t i ) const ;
bool use_for_yaw ( void ) const ;
2012-01-12 17:43:39 -04:00
2012-08-21 23:19:51 -03:00
/// Sets the local magnetic field declination.
///
/// @param radians Local field declination.
2013-04-15 09:50:44 -03:00
/// @param save_to_eeprom true to save to eeprom (false saves only to memory)
2012-08-21 23:19:51 -03:00
///
2013-04-15 09:50:44 -03:00
void set_declination ( float radians , bool save_to_eeprom = true ) ;
2013-05-08 20:22:00 -03:00
float get_declination ( ) const ;
2011-02-14 00:27:07 -04:00
2013-01-13 01:02:49 -04:00
// set overall board orientation
void set_board_orientation ( enum Rotation orientation ) {
_board_orientation = orientation ;
}
2013-03-03 10:02:12 -04:00
/// Set the motor compensation type
///
/// @param comp_type 0 = disabled, 1 = enabled use throttle, 2 = enabled use current
///
void motor_compensation_type ( const uint8_t comp_type ) {
2013-05-24 08:02:49 -03:00
if ( _motor_comp_type < = AP_COMPASS_MOT_COMP_CURRENT & & _motor_comp_type ! = ( int8_t ) comp_type ) {
2013-03-03 10:02:12 -04:00
_motor_comp_type = ( int8_t ) comp_type ;
_thr_or_curr = 0 ; // set current current or throttle to zero
2014-07-22 04:29:43 -03:00
for ( uint8_t i = 0 ; i < COMPASS_MAX_INSTANCES ; i + + ) {
set_motor_compensation ( i , Vector3f ( 0 , 0 , 0 ) ) ; // clear out invalid compensation vectors
}
2013-03-03 10:02:12 -04:00
}
}
/// get the motor compensation value.
2013-05-08 20:22:00 -03:00
uint8_t motor_compensation_type ( ) const {
2013-03-03 10:02:12 -04:00
return _motor_comp_type ;
}
2013-02-27 03:57:04 -04:00
/// Set the motor compensation factor x/y/z values.
///
2014-07-22 04:29:43 -03:00
/// @param i instance of compass
2013-02-27 03:57:04 -04:00
/// @param offsets Offsets multiplied by the throttle value and added to the raw mag_ values.
///
2014-07-22 04:29:43 -03:00
void set_motor_compensation ( uint8_t i , const Vector3f & motor_comp_factor ) ;
2013-02-27 03:57:04 -04:00
2013-03-02 04:53:03 -04:00
/// get motor compensation factors as a vector
2013-12-09 04:45:31 -04:00
const Vector3f & get_motor_compensation ( uint8_t i ) const { return _motor_compensation [ i ] ; }
2014-07-22 04:30:33 -03:00
const Vector3f & get_motor_compensation ( void ) const { return get_motor_compensation ( get_primary ( ) ) ; }
2013-02-27 03:57:04 -04:00
/// Saves the current motor compensation x/y/z values.
///
/// This should be invoked periodically to save the offset values calculated by the motor compensation auto learning
///
2013-03-01 11:00:37 -04:00
void save_motor_compensation ( ) ;
2013-02-27 03:57:04 -04:00
/// Returns the current motor compensation offset values
///
/// @returns The current compass offsets.
///
2013-12-09 04:45:31 -04:00
const Vector3f & get_motor_offsets ( uint8_t i ) const { return _motor_offset [ i ] ; }
2014-07-22 04:30:33 -03:00
const Vector3f & get_motor_offsets ( void ) const { return get_motor_offsets ( get_primary ( ) ) ; }
2013-02-27 03:57:04 -04:00
/// Set the throttle as a percentage from 0.0 to 1.0
/// @param thr_pct throttle expressed as a percentage from 0 to 1.0
void set_throttle ( float thr_pct ) {
2013-03-03 10:02:12 -04:00
if ( _motor_comp_type = = AP_COMPASS_MOT_COMP_THROTTLE ) {
_thr_or_curr = thr_pct ;
}
}
/// Set the current used by system in amps
/// @param amps current flowing to the motors expressed in amps
void set_current ( float amps ) {
if ( _motor_comp_type = = AP_COMPASS_MOT_COMP_CURRENT ) {
_thr_or_curr = amps ;
}
2013-02-27 03:57:04 -04:00
}
2014-07-07 09:24:18 -03:00
/// Returns True if the compasses have been configured (i.e. offsets saved)
///
/// @returns True if compass has been configured
///
bool configured ( uint8_t i ) ;
bool configured ( void ) ;
2014-07-22 04:30:33 -03:00
/// Returns the instance of the primary compass
///
/// @returns the instance number of the primary compass
///
virtual uint8_t get_primary ( void ) const { return 0 ; }
2013-02-27 03:57:04 -04:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
2013-04-16 06:47:39 -03:00
// settable parameters
AP_Int8 _learn ; ///<enable calibration learning
2011-02-14 00:27:07 -04:00
protected :
2013-12-09 05:01:42 -04:00
2013-12-09 02:46:41 -04:00
bool _healthy [ COMPASS_MAX_INSTANCES ] ;
2013-12-09 01:05:57 -04:00
Vector3f _field [ COMPASS_MAX_INSTANCES ] ; ///< magnetic field strength
2013-12-08 23:09:34 -04:00
2014-09-24 10:26:42 -03:00
AP_Int8 _orientation [ COMPASS_MAX_INSTANCES ] ;
2013-12-09 02:33:54 -04:00
AP_Vector3f _offset [ COMPASS_MAX_INSTANCES ] ;
2012-08-21 23:19:51 -03:00
AP_Float _declination ;
2014-09-18 09:56:13 -03:00
AP_Int8 _use_for_yaw [ COMPASS_MAX_INSTANCES ] ; ///<enable use for yaw calculation
2012-08-21 23:19:51 -03:00
AP_Int8 _auto_declination ; ///<enable automatic declination code
2014-09-24 10:26:42 -03:00
AP_Int8 _external [ COMPASS_MAX_INSTANCES ] ; ///<compass is external
2014-05-21 23:52:25 -03:00
# if COMPASS_MAX_INSTANCES > 1
AP_Int8 _primary ; ///primary instance
2014-07-07 09:24:18 -03:00
AP_Int32 _dev_id [ COMPASS_MAX_INSTANCES ] ; // device id detected at init. saved to eeprom when offsets are saved allowing ram & eeprom values to be compared as consistency check
2014-05-21 23:52:25 -03:00
# endif
2011-02-14 00:27:07 -04:00
2012-08-21 23:19:51 -03:00
bool _null_init_done ; ///< first-time-around flag used by offset nulling
2012-03-28 06:46:11 -03:00
///< used by offset correction
static const uint8_t _mag_history_size = 20 ;
2013-12-09 02:33:54 -04:00
uint8_t _mag_history_index [ COMPASS_MAX_INSTANCES ] ;
Vector3i _mag_history [ COMPASS_MAX_INSTANCES ] [ _mag_history_size ] ;
2013-01-13 01:02:49 -04:00
2013-02-27 03:57:04 -04:00
// motor compensation
2013-03-03 10:02:12 -04:00
AP_Int8 _motor_comp_type ; // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
2013-12-09 04:45:31 -04:00
AP_Vector3f _motor_compensation [ COMPASS_MAX_INSTANCES ] ; // factors multiplied by throttle and added to compass outputs
Vector3f _motor_offset [ COMPASS_MAX_INSTANCES ] ; // latest compensation added to compass
2013-03-03 10:02:12 -04:00
float _thr_or_curr ; // throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps
2013-02-27 03:57:04 -04:00
2013-01-13 01:02:49 -04:00
// board orientation from AHRS
enum Rotation _board_orientation ;
2014-07-31 22:19:53 -03:00
void apply_corrections ( Vector3f & mag , uint8_t i ) ;
2011-02-14 00:27:07 -04:00
} ;
# endif