2011-12-21 00:30:22 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2011-11-12 23:20:25 -04:00
# ifndef __AP_INERTIAL_SENSOR_H__
# define __AP_INERTIAL_SENSOR_H__
2012-11-05 00:27:03 -04:00
// Gyro and Accelerometer calibration criteria
2013-01-10 14:42:24 -04:00
# define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f
# define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f
2012-11-05 00:27:03 -04:00
2013-12-08 18:50:12 -04:00
/**
maximum number of INS instances available on this platform . If more
than 1 then redundent sensors may be available
*/
2014-02-22 17:16:33 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
2013-12-08 18:50:12 -04:00
# define INS_MAX_INSTANCES 2
# else
# define INS_MAX_INSTANCES 1
# endif
2012-10-11 21:27:19 -03:00
# include <stdint.h>
# include <AP_HAL.h>
# include <AP_Math.h>
2012-12-19 18:02:34 -04:00
# include "AP_InertialSensor_UserInteract.h"
2011-11-12 23:20:25 -04:00
/* AP_InertialSensor is an abstraction for gyro and accel measurements
* which are correctly aligned to the body axes and scaled to SI units .
2012-12-04 20:20:31 -04:00
*
* Gauss - Newton accel calibration routines borrowed from Rolfe Schmidt
* blog post describing the method : http : //chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
* original sketch available at http : //rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
2011-11-12 23:20:25 -04:00
*/
class AP_InertialSensor
{
2012-08-17 03:19:56 -03:00
public :
2012-12-19 00:55:38 -04:00
AP_InertialSensor ( ) ;
2012-11-05 00:27:03 -04:00
enum Start_style {
COLD_START = 0 ,
WARM_START
} ;
2012-11-29 07:56:13 -04:00
// the rate that updates will be available to the application
enum Sample_rate {
RATE_50HZ ,
RATE_100HZ ,
2013-12-16 23:24:44 -04:00
RATE_200HZ ,
RATE_400HZ
2012-11-29 07:56:13 -04:00
} ;
2012-11-05 00:27:03 -04:00
/// Perform startup initialisation.
///
/// Called to initialise the state of the IMU.
///
/// For COLD_START, implementations using real sensors can assume
/// that the airframe is stationary and nominally oriented.
///
/// For WARM_START, no assumptions should be made about the
/// orientation or motion of the airframe. Calibration should be
/// as for the previous COLD_START call.
///
/// @param style The initialisation startup style.
///
2012-10-11 21:27:19 -03:00
virtual void init ( Start_style style ,
2013-09-19 05:32:19 -03:00
Sample_rate sample_rate ) ;
2012-11-05 00:27:03 -04:00
/// Perform cold startup initialisation for just the accelerometers.
///
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work.
///
2013-09-19 05:32:19 -03:00
virtual void init_accel ( ) ;
2012-11-05 00:27:03 -04:00
2012-11-20 03:41:04 -04:00
# if !defined( __AVR_ATmega1280__ )
2012-10-11 21:27:19 -03:00
// perform accelerometer calibration including providing user instructions
// and feedback
2013-09-19 05:32:19 -03:00
virtual bool calibrate_accel ( AP_InertialSensor_UserInteract * interact ,
2013-01-30 07:47:57 -04:00
float & trim_roll ,
float & trim_pitch ) ;
2012-11-20 03:41:04 -04:00
# endif
2012-11-05 00:27:03 -04:00
2013-04-22 11:55:53 -03:00
/// calibrated - returns true if the accelerometers have been calibrated
///
/// @note this should not be called while flying because it reads from the eeprom which can be slow
///
bool calibrated ( ) ;
2012-11-05 00:27:03 -04:00
/// Perform cold-start initialisation for just the gyros.
///
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work
///
2013-09-19 05:32:19 -03:00
virtual void init_gyro ( void ) ;
2012-11-05 00:27:03 -04:00
/// Fetch the current gyro values
///
/// @returns vector of rotational rates in radians/sec
///
2013-12-08 18:50:12 -04:00
const Vector3f & get_gyro ( uint8_t i ) const { return _gyro [ i ] ; }
2013-12-09 05:02:04 -04:00
const Vector3f & get_gyro ( void ) const { return get_gyro ( _get_primary_gyro ( ) ) ; }
2014-02-22 17:16:33 -04:00
virtual void set_gyro ( uint8_t instance , const Vector3f & gyro ) { }
2012-11-05 00:27:03 -04:00
// set gyro offsets in radians/sec
2013-12-08 18:50:12 -04:00
const Vector3f & get_gyro_offsets ( uint8_t i ) const { return _gyro_offset [ i ] ; }
2013-12-09 05:02:04 -04:00
const Vector3f & get_gyro_offsets ( void ) const { return get_gyro_offsets ( _get_primary_gyro ( ) ) ; }
2012-11-05 00:27:03 -04:00
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
///
2013-12-08 18:50:12 -04:00
const Vector3f & get_accel ( uint8_t i ) const { return _accel [ i ] ; }
2013-12-09 05:02:04 -04:00
const Vector3f & get_accel ( void ) const { return get_accel ( _get_primary_accel ( ) ) ; }
2014-02-22 17:16:33 -04:00
virtual void set_accel ( uint8_t instance , const Vector3f & accel ) { }
2012-11-05 00:27:03 -04:00
2013-12-08 05:43:53 -04:00
// multi-device interface
2014-01-22 07:08:28 -04:00
virtual bool get_gyro_health ( uint8_t instance ) const { return true ; }
2013-12-09 05:02:04 -04:00
bool get_gyro_health ( void ) const { return get_gyro_health ( _get_primary_gyro ( ) ) ; }
2013-12-08 05:43:53 -04:00
virtual uint8_t get_gyro_count ( void ) const { return 1 ; } ;
2014-01-22 07:08:28 -04:00
virtual bool get_accel_health ( uint8_t instance ) const { return true ; }
2013-12-09 05:02:04 -04:00
bool get_accel_health ( void ) const { return get_accel_health ( _get_primary_accel ( ) ) ; }
2013-12-08 05:43:53 -04:00
virtual uint8_t get_accel_count ( void ) const { return 1 ; } ;
2012-11-05 00:27:03 -04:00
// get accel offsets in m/s/s
2013-12-08 18:50:12 -04:00
const Vector3f & get_accel_offsets ( uint8_t i ) const { return _accel_offset [ i ] ; }
2013-12-09 05:02:04 -04:00
const Vector3f & get_accel_offsets ( void ) const { return get_accel_offsets ( _get_primary_accel ( ) ) ; }
2012-11-05 00:27:03 -04:00
// get accel scale
2013-12-08 18:50:12 -04:00
const Vector3f & get_accel_scale ( uint8_t i ) const { return _accel_scale [ i ] ; }
2013-12-09 05:02:04 -04:00
const Vector3f & get_accel_scale ( void ) const { return get_accel_scale ( _get_primary_accel ( ) ) ; }
2012-11-05 00:27:03 -04:00
2012-08-17 03:19:56 -03:00
/* Update the sensor data, so that getters are nonblocking.
* Returns a bool of whether data was updated or not .
*/
2012-10-11 21:27:19 -03:00
virtual bool update ( ) = 0 ;
2012-08-17 03:19:56 -03:00
2012-11-05 00:27:03 -04:00
/* get_delta_time returns the time period in seconds
2012-08-30 04:48:36 -03:00
* overwhich the sensor data was collected
2012-08-17 03:19:56 -03:00
*/
2014-01-02 20:46:24 -04:00
virtual float get_delta_time ( ) const = 0 ;
2012-08-17 03:19:56 -03:00
// return the maximum gyro drift rate in radians/s/s. This
// depends on what gyro chips are being used
2012-10-11 21:27:19 -03:00
virtual float get_gyro_drift_rate ( void ) = 0 ;
2012-03-08 03:10:27 -04:00
2013-10-08 03:28:39 -03:00
// wait for a sample to be available, with timeout in milliseconds
virtual bool wait_for_sample ( uint16_t timeout_ms ) = 0 ;
2012-11-05 00:27:03 -04:00
// class level parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
2013-01-13 01:03:13 -04:00
// set overall board orientation
void set_board_orientation ( enum Rotation orientation ) {
_board_orientation = orientation ;
}
2013-09-09 06:18:52 -03:00
// override default filter frequency
void set_default_filter ( float filter_hz ) {
if ( ! _mpu6000_filter . load ( ) ) {
_mpu6000_filter . set ( filter_hz ) ;
}
}
2013-10-29 18:54:03 -03:00
virtual uint16_t error_count ( void ) const { return 0 ; }
2013-12-09 05:02:04 -04:00
virtual bool healthy ( void ) const { return get_gyro_health ( ) & & get_accel_health ( ) ; }
2013-10-29 18:54:03 -03:00
2012-11-05 00:27:03 -04:00
protected :
2013-12-09 05:02:04 -04:00
virtual uint8_t _get_primary_gyro ( void ) const { return 0 ; }
virtual uint8_t _get_primary_accel ( void ) const { return 0 ; }
2012-11-07 02:20:22 -04:00
// sensor specific init to be overwritten by descendant classes
2012-10-11 21:27:19 -03:00
virtual uint16_t _init_sensor ( Sample_rate sample_rate ) = 0 ;
2012-11-07 02:20:22 -04:00
2012-11-05 00:27:03 -04:00
// no-save implementations of accel and gyro initialisation routines
2013-09-19 05:32:19 -03:00
virtual void _init_accel ( ) ;
2012-10-11 21:27:19 -03:00
2013-09-19 05:32:19 -03:00
virtual void _init_gyro ( ) ;
2012-11-05 00:27:03 -04:00
2012-11-20 03:41:04 -04:00
# if !defined( __AVR_ATmega1280__ )
2012-12-04 20:20:31 -04:00
// Calibration routines borrowed from Rolfe Schmidt
// blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
2012-11-05 00:27:03 -04:00
// _calibrate_accel - perform low level accel calibration
2012-11-07 02:20:22 -04:00
virtual bool _calibrate_accel ( Vector3f accel_sample [ 6 ] , Vector3f & accel_offsets , Vector3f & accel_scale ) ;
2012-11-05 00:27:03 -04:00
virtual void _calibrate_update_matrices ( float dS [ 6 ] , float JS [ 6 ] [ 6 ] , float beta [ 6 ] , float data [ 3 ] ) ;
virtual void _calibrate_reset_matrices ( float dS [ 6 ] , float JS [ 6 ] [ 6 ] ) ;
virtual void _calibrate_find_delta ( float dS [ 6 ] , float JS [ 6 ] [ 6 ] , float delta [ 6 ] ) ;
2013-01-30 07:47:57 -04:00
virtual void _calculate_trim ( Vector3f accel_sample , float & trim_roll , float & trim_pitch ) ;
2012-11-20 03:41:04 -04:00
# endif
2012-11-05 00:27:03 -04:00
// save parameters to eeprom
2012-10-11 21:27:19 -03:00
void _save_parameters ( ) ;
2012-11-05 00:27:03 -04:00
// Most recent accelerometer reading obtained by ::update
2013-12-08 18:50:12 -04:00
Vector3f _accel [ INS_MAX_INSTANCES ] ;
2012-11-05 00:27:03 -04:00
2013-11-06 22:53:59 -04:00
// previous accelerometer reading obtained by ::update
2013-12-08 18:50:12 -04:00
Vector3f _previous_accel [ INS_MAX_INSTANCES ] ;
2013-11-06 22:53:59 -04:00
2012-11-05 00:27:03 -04:00
// Most recent gyro reading obtained by ::update
2013-12-08 18:50:12 -04:00
Vector3f _gyro [ INS_MAX_INSTANCES ] ;
2012-11-05 00:27:03 -04:00
// product id
2012-10-11 21:27:19 -03:00
AP_Int16 _product_id ;
2012-08-30 04:48:36 -03:00
2012-11-05 00:27:03 -04:00
// accelerometer scaling and offsets
2013-12-08 18:50:12 -04:00
AP_Vector3f _accel_scale [ INS_MAX_INSTANCES ] ;
AP_Vector3f _accel_offset [ INS_MAX_INSTANCES ] ;
AP_Vector3f _gyro_offset [ INS_MAX_INSTANCES ] ;
2012-11-29 16:15:12 -04:00
// filtering frequency (0 means default)
AP_Int8 _mpu6000_filter ;
2013-01-13 01:03:13 -04:00
// board orientation from AHRS
enum Rotation _board_orientation ;
2011-11-12 23:20:25 -04:00
} ;
# include "AP_InertialSensor_Oilpan.h"
# include "AP_InertialSensor_MPU6000.h"
2013-09-28 06:33:21 -03:00
# include "AP_InertialSensor_HIL.h"
2013-01-03 23:25:57 -04:00
# include "AP_InertialSensor_PX4.h"
2012-12-19 18:26:19 -04:00
# include "AP_InertialSensor_UserInteract_Stream.h"
2013-05-08 03:18:40 -03:00
# include "AP_InertialSensor_UserInteract_MAVLink.h"
2013-09-23 00:32:19 -03:00
# include "AP_InertialSensor_Flymaple.h"
2013-10-07 21:07:59 -03:00
# include "AP_InertialSensor_L3G4200D.h"
2011-11-12 23:20:25 -04:00
# endif // __AP_INERTIAL_SENSOR_H__