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-10-14 01:48:33 -03:00
# if HAL_CPU_CLASS > HAL_CPU_CLASS_16
2014-07-14 07:53:59 -03:00
# define INS_MAX_INSTANCES 3
2014-10-15 20:32:40 -03:00
# define INS_MAX_BACKENDS 6
2013-12-08 18:50:12 -04:00
# else
# define INS_MAX_INSTANCES 1
2014-10-15 20:32:40 -03:00
# define INS_MAX_BACKENDS 1
2013-12-08 18:50:12 -04:00
# endif
2014-10-15 20:32:40 -03:00
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"
2014-10-14 01:48:33 -03:00
class AP_InertialSensor_Backend ;
2015-05-06 23:08:30 -03:00
/*
forward declare DataFlash class . We can ' t include DataFlash . h
because of mutual dependencies
*/
class DataFlash_Class ;
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
{
2014-10-14 01:48:33 -03:00
friend class AP_InertialSensor_Backend ;
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 {
2015-03-11 20:02:36 -03:00
RATE_50HZ = 50 ,
RATE_100HZ = 100 ,
RATE_200HZ = 200 ,
RATE_400HZ = 400
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.
///
2014-10-14 01:48:33 -03:00
void init ( Start_style style ,
Sample_rate sample_rate ) ;
2012-11-05 00:27:03 -04:00
2014-10-14 01:48:33 -03:00
/// Register a new gyro/accel driver, allocating an instance
/// number
uint8_t register_gyro ( void ) ;
uint8_t register_accel ( void ) ;
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
2014-10-14 01:48:33 -03:00
bool calibrate_accel ( AP_InertialSensor_UserInteract * interact ,
float & trim_roll ,
float & trim_pitch ) ;
2012-11-20 03:41:04 -04:00
# endif
2015-05-15 18:22:25 -03:00
bool calibrate_trim ( float & trim_roll , float & trim_pitch ) ;
2012-11-05 00:27:03 -04:00
2015-03-09 03:31:55 -03:00
/// calibrating - returns true if the gyros or accels are currently being calibrated
bool calibrating ( ) const { return _calibrating ; }
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
///
2014-10-14 01:48:33 -03:00
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 ] ; }
2014-10-14 01:48:33 -03:00
const Vector3f & get_gyro ( void ) const { return get_gyro ( _primary_gyro ) ; }
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 ] ; }
2014-10-14 01:48:33 -03:00
const Vector3f & get_gyro_offsets ( void ) const { return get_gyro_offsets ( _primary_gyro ) ; }
2012-11-05 00:27:03 -04:00
2015-02-17 02:54:17 -04:00
//get delta angle if available
bool get_delta_angle ( uint8_t i , Vector3f & delta_angle ) const {
if ( _delta_angle_valid [ i ] ) delta_angle = _delta_angle [ i ] ;
return _delta_angle_valid [ i ] ;
}
bool get_delta_angle ( Vector3f & delta_angle ) const { return get_delta_angle ( _primary_gyro , delta_angle ) ; }
//get delta velocity if available
bool get_delta_velocity ( uint8_t i , Vector3f & delta_velocity ) const {
if ( _delta_velocity_valid [ i ] ) delta_velocity = _delta_velocity [ i ] ;
return _delta_velocity_valid [ i ] ;
}
bool get_delta_velocity ( Vector3f & delta_velocity ) const { return get_delta_velocity ( _primary_accel , delta_velocity ) ; }
2015-03-21 17:18:58 -03:00
float get_delta_velocity_dt ( uint8_t i ) const {
return _delta_velocity_dt [ i ] ;
}
float get_delta_velocity ( ) const { return get_delta_velocity_dt ( _primary_accel ) ; }
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 ] ; }
2014-10-14 01:48:33 -03:00
const Vector3f & get_accel ( void ) const { return get_accel ( _primary_accel ) ; }
void set_accel ( uint8_t instance , const Vector3f & accel ) ;
2012-11-05 00:27:03 -04:00
2014-12-29 06:19:35 -04:00
uint32_t get_gyro_error_count ( uint8_t i ) const { return _gyro_error_count [ i ] ; }
uint32_t get_accel_error_count ( uint8_t i ) const { return _accel_error_count [ i ] ; }
2013-12-08 05:43:53 -04:00
// multi-device interface
2015-04-17 00:34:33 -03:00
bool get_gyro_health ( uint8_t instance ) const { return ( instance < _gyro_count ) ? _gyro_healthy [ instance ] : false ; }
2014-10-14 01:48:33 -03:00
bool get_gyro_health ( void ) const { return get_gyro_health ( _primary_gyro ) ; }
2014-09-01 08:20:27 -03:00
bool get_gyro_health_all ( void ) const ;
2014-10-14 01:48:33 -03:00
uint8_t get_gyro_count ( void ) const { return _gyro_count ; }
2014-10-08 08:17:31 -03:00
bool gyro_calibrated_ok ( uint8_t instance ) const { return _gyro_cal_ok [ instance ] ; }
bool gyro_calibrated_ok_all ( ) const ;
2013-12-08 05:43:53 -04:00
2015-04-17 00:34:33 -03:00
bool get_accel_health ( uint8_t instance ) const { return ( instance < _accel_count ) ? _accel_healthy [ instance ] : false ; }
2014-10-14 01:48:33 -03:00
bool get_accel_health ( void ) const { return get_accel_health ( _primary_accel ) ; }
2014-09-01 08:20:27 -03:00
bool get_accel_health_all ( void ) const ;
2014-10-14 01:48:33 -03:00
uint8_t get_accel_count ( void ) const { return _accel_count ; } ;
2015-05-12 03:18:25 -03:00
bool accel_calibrated_ok_all ( ) const ;
2013-12-08 05:43:53 -04:00
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 ] ; }
2014-10-14 01:48:33 -03:00
const Vector3f & get_accel_offsets ( void ) const { return get_accel_offsets ( _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 ] ; }
2014-10-14 01:48:33 -03:00
const Vector3f & get_accel_scale ( void ) const { return get_accel_scale ( _primary_accel ) ; }
2012-08-17 03:19:56 -03:00
2015-03-16 23:32:54 -03:00
// return the temperature if supported. Zero is returned if no
// temperature is available
float get_temperature ( uint8_t instance ) const { return _temperature [ instance ] ; }
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-10-14 01:48:33 -03:00
float get_delta_time ( ) const { return _delta_time ; }
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
2014-10-14 01:48:33 -03:00
float get_gyro_drift_rate ( void ) const { return ToRad ( 0.5f / 60 ) ; }
2012-03-08 03:10:27 -04:00
2014-10-14 01:48:33 -03:00
// update gyro and accel values from accumulated samples
void update ( void ) ;
// wait for a sample to be available
void wait_for_sample ( void ) ;
2013-10-08 03:28:39 -03:00
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 ;
}
2014-10-16 17:52:21 -03:00
// return the selected sample rate
Sample_rate get_sample_rate ( void ) const { return _sample_rate ; }
2014-10-14 01:48:33 -03:00
uint16_t error_count ( void ) const { return 0 ; }
bool healthy ( void ) const { return get_gyro_health ( ) & & get_accel_health ( ) ; }
2012-11-05 00:27:03 -04:00
2015-04-16 20:41:54 -03:00
uint8_t get_primary_accel ( void ) const { return _primary_accel ; }
uint8_t get_primary_gyro ( void ) const { return _primary_gyro ; }
2013-12-09 05:02:04 -04:00
2014-10-15 20:32:40 -03:00
// enable HIL mode
void set_hil_mode ( void ) { _hil_mode = true ; }
2015-03-11 22:19:31 -03:00
// get the gyro filter rate in Hz
uint8_t get_gyro_filter_hz ( void ) const { return _gyro_filter_cutoff ; }
// get the accel filter rate in Hz
uint8_t get_accel_filter_hz ( void ) const { return _accel_filter_cutoff ; }
2015-05-06 23:08:30 -03:00
// pass in a pointer to DataFlash for raw data logging
void set_dataflash ( DataFlash_Class * dataflash ) { _dataflash = dataflash ; }
// enable/disable raw gyro/accel logging
void set_raw_logging ( bool enable ) { _log_raw_data = enable ; }
2014-10-14 01:48:33 -03:00
private :
2012-11-07 02:20:22 -04:00
2014-10-14 01:48:33 -03:00
// load backend drivers
2014-10-16 17:52:21 -03:00
void _add_backend ( AP_InertialSensor_Backend * ( detect ) ( AP_InertialSensor & ) ) ;
void _detect_backends ( void ) ;
2012-10-11 21:27:19 -03:00
2015-03-10 20:16:04 -03:00
// gyro initialisation
2014-10-14 01:48:33 -03:00
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
2015-03-10 19:40:02 -03:00
bool _calibrate_accel ( const Vector3f accel_sample [ 6 ] , Vector3f & accel_offsets , Vector3f & accel_scale , enum Rotation r ) ;
bool _check_sample_range ( const Vector3f accel_sample [ 6 ] , enum Rotation rotation ,
AP_InertialSensor_UserInteract * interact ) ;
2014-10-14 01:48:33 -03:00
void _calibrate_update_matrices ( float dS [ 6 ] , float JS [ 6 ] [ 6 ] , float beta [ 6 ] , float data [ 3 ] ) ;
void _calibrate_reset_matrices ( float dS [ 6 ] , float JS [ 6 ] [ 6 ] ) ;
void _calibrate_find_delta ( float dS [ 6 ] , float JS [ 6 ] [ 6 ] , float delta [ 6 ] ) ;
2015-05-15 18:18:09 -03:00
bool _calculate_trim ( const 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
2014-10-14 01:48:33 -03:00
// backend objects
2014-10-15 20:32:40 -03:00
AP_InertialSensor_Backend * _backends [ INS_MAX_BACKENDS ] ;
2012-11-05 00:27:03 -04:00
2014-10-14 01:48:33 -03:00
// number of gyros and accel drivers. Note that most backends
// provide both accel and gyro data, so will increment both
// counters on initialisation
uint8_t _gyro_count ;
uint8_t _accel_count ;
2014-10-16 17:27:01 -03:00
uint8_t _backend_count ;
2013-11-06 22:53:59 -04:00
2014-10-16 17:52:21 -03:00
// the selected sample rate
Sample_rate _sample_rate ;
2014-10-14 01:48:33 -03:00
// Most recent accelerometer reading
Vector3f _accel [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
Vector3f _delta_velocity [ INS_MAX_INSTANCES ] ;
2015-03-21 17:18:58 -03:00
float _delta_velocity_dt [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
bool _delta_velocity_valid [ INS_MAX_INSTANCES ] ;
2014-10-14 01:48:33 -03:00
// Most recent gyro reading
2013-12-08 18:50:12 -04:00
Vector3f _gyro [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
Vector3f _delta_angle [ INS_MAX_INSTANCES ] ;
bool _delta_angle_valid [ 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
2014-10-14 01:48:33 -03: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
2015-03-16 23:32:54 -03:00
// temperatures for an instance if available
float _temperature [ INS_MAX_INSTANCES ] ;
2012-11-29 16:15:12 -04:00
// filtering frequency (0 means default)
2015-03-11 21:58:36 -03:00
AP_Int8 _accel_filter_cutoff ;
AP_Int8 _gyro_filter_cutoff ;
2013-01-13 01:03:13 -04:00
// board orientation from AHRS
2014-10-14 01:48:33 -03:00
enum Rotation _board_orientation ;
2014-10-08 08:17:31 -03:00
// calibrated_ok flags
2014-10-14 01:48:33 -03:00
bool _gyro_cal_ok [ INS_MAX_INSTANCES ] ;
// primary accel and gyro
uint8_t _primary_gyro ;
uint8_t _primary_accel ;
2014-10-15 20:32:40 -03:00
// has wait_for_sample() found a sample?
bool _have_sample : 1 ;
// are we in HIL mode?
bool _hil_mode : 1 ;
2015-03-09 03:31:55 -03:00
// are gyros or accels currently being calibrated
bool _calibrating : 1 ;
2015-05-06 23:08:30 -03:00
// should we log raw accel/gyro data?
bool _log_raw_data : 1 ;
2014-10-15 20:32:40 -03:00
// the delta time in seconds for the last sample
2014-10-14 01:48:33 -03:00
float _delta_time ;
2014-10-15 05:54:30 -03:00
// last time a wait_for_sample() returned a sample
uint32_t _last_sample_usec ;
2014-10-19 20:46:02 -03:00
// target time for next wait_for_sample() return
uint32_t _next_sample_usec ;
2014-10-15 05:54:30 -03:00
// time between samples in microseconds
uint32_t _sample_period_usec ;
2014-10-15 23:27:22 -03:00
// health of gyros and accels
bool _gyro_healthy [ INS_MAX_INSTANCES ] ;
bool _accel_healthy [ INS_MAX_INSTANCES ] ;
2014-12-29 06:19:35 -04:00
uint32_t _accel_error_count [ INS_MAX_INSTANCES ] ;
uint32_t _gyro_error_count [ INS_MAX_INSTANCES ] ;
2015-05-06 23:08:30 -03:00
DataFlash_Class * _dataflash ;
2011-11-12 23:20:25 -04:00
} ;
2014-10-14 01:48:33 -03:00
# include "AP_InertialSensor_Backend.h"
2011-11-12 23:20:25 -04:00
# include "AP_InertialSensor_MPU6000.h"
2014-10-15 05:54:30 -03:00
# include "AP_InertialSensor_PX4.h"
2014-10-15 20:32:40 -03:00
# include "AP_InertialSensor_Oilpan.h"
# include "AP_InertialSensor_MPU9250.h"
2014-10-15 20:55:18 -03:00
# include "AP_InertialSensor_L3G4200D.h"
2014-10-15 21:24:32 -03:00
# include "AP_InertialSensor_Flymaple.h"
2014-10-15 22:03:28 -03:00
# include "AP_InertialSensor_MPU9150.h"
2014-10-15 02:37:59 -03:00
# include "AP_InertialSensor_HIL.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"
2011-11-12 23:20:25 -04:00
# endif // __AP_INERTIAL_SENSOR_H__