2012-11-05 00:31:02 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2012-11-07 09:24:00 -04:00
# ifndef __AP_INERTIALNAV_H__
# define __AP_INERTIALNAV_H__
2012-11-05 00:31:02 -04:00
# include <AP_AHRS.h>
# include <AP_InertialSensor.h> // ArduPilot Mega IMU Library
# include <AP_Baro.h> // ArduPilot Mega Barometer Library
2012-12-09 11:43:11 -04:00
# include <AP_Buffer.h> // FIFO buffer library
2013-09-19 03:56:23 -03:00
# include <AP_GPS_Glitch.h> // GPS Glitch detection library
2014-07-28 05:36:46 -03:00
# include <AP_Baro_Glitch.h> // Baro Glitch detection library
2012-11-05 00:31:02 -04:00
2013-04-30 00:52:28 -03:00
# define AP_INTERTIALNAV_TC_XY 2.5f // default time constant for complementary filter's X & Y axis
2013-03-31 23:51:12 -03:00
# define AP_INTERTIALNAV_TC_Z 5.0f // default time constant for complementary filter's Z axis
2013-01-10 03:56:21 -04:00
2012-12-09 11:43:11 -04:00
// #defines to control how often historical accel based positions are saved
// so they can later be compared to laggy gps readings
# define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10
# define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y
2013-01-22 05:16:18 -04:00
# define AP_INTERTIALNAV_GPS_TIMEOUT_MS 300 // timeout after which position error from GPS will fall to zero
2012-12-09 11:43:11 -04:00
2012-11-05 00:31:02 -04:00
/*
2013-10-25 10:55:31 -03:00
* AP_InertialNav blends accelerometer data with gps and barometer data to improve altitude and position hold .
*
* Most of the functions have to be called at 100 Hz . ( see defines above )
*
* The accelerometer values are integrated over time to approximate velocity and position .
* The inaccurcy of these estimates grows over time due to noisy sensor data .
* To improve the accuracy , baro and gps readings are used :
* An error value is calculated as the difference between the sensor ' s measurement and the last position estimation .
* This value is weighted with a gain factor and incorporated into the new estimation
2012-11-05 00:31:02 -04:00
*/
2012-11-07 09:24:00 -04:00
class AP_InertialNav
2012-11-05 00:31:02 -04:00
{
public :
// Constructor
2014-07-28 05:36:46 -03:00
AP_InertialNav ( AP_AHRS & ahrs , AP_Baro & baro , GPS_Glitch & gps_glitch , Baro_Glitch & baro_glitch ) :
2012-11-05 00:31:02 -04:00
_ahrs ( ahrs ) ,
_baro ( baro ) ,
_xy_enabled ( false ) ,
2014-07-14 22:54:59 -03:00
_k1_xy ( 0.0f ) ,
_k2_xy ( 0.0f ) ,
_k3_xy ( 0.0f ) ,
2012-12-09 11:43:11 -04:00
_gps_last_update ( 0 ) ,
2013-04-17 10:00:52 -03:00
_gps_last_time ( 0 ) ,
2013-08-07 11:00:09 -03:00
_historic_xy_counter ( 0 ) ,
2014-07-14 22:54:59 -03:00
_lon_to_cm_scaling ( LATLON_TO_CM ) ,
_k1_z ( 0.0f ) ,
_k2_z ( 0.0f ) ,
_k3_z ( 0.0f ) ,
2013-09-19 03:56:23 -03:00
_baro_last_update ( 0 ) ,
2013-11-21 02:32:16 -04:00
_glitch_detector ( gps_glitch ) ,
2014-07-28 05:36:46 -03:00
_baro_glitch ( baro_glitch ) ,
2013-11-21 02:32:16 -04:00
_error_count ( 0 )
2012-12-12 17:46:06 -04:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
* initializes the object .
*/
2014-01-03 20:16:07 -04:00
virtual void init ( ) ;
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
* update - updates velocity and position estimates using latest info from accelerometers
* augmented with gps and baro readings
*
* @ param dt : time since last update in seconds
*/
2014-01-03 20:16:07 -04:00
virtual void update ( float dt ) ;
2012-11-05 00:31:02 -04:00
2012-12-09 11:43:11 -04:00
//
// XY Axis specific methods
//
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
* position_ok - true if inertial based altitude and position can be trusted
* @ return
*/
2014-01-03 20:16:07 -04:00
virtual bool position_ok ( ) const ;
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
* get_position - returns the current position relative to the home location in cm .
*
* @ return
*/
2014-01-03 20:16:07 -04:00
virtual const Vector3f & get_position ( ) const { return _position ; }
2013-10-25 10:55:31 -03:00
/**
* get_latitude - returns the latitude of the current position estimation in 100 nano degrees ( i . e . degree value multiplied by 10 , 000 , 000 )
* @ return
*/
2014-01-03 20:16:07 -04:00
virtual int32_t get_latitude ( ) const ;
2013-10-25 10:55:31 -03:00
/**
* get_longitude - returns the longitude of the current position estimation in 100 nano degrees ( i . e . degree value multiplied by 10 , 000 , 000 )
* @ return
*/
2014-01-03 20:16:07 -04:00
virtual int32_t get_longitude ( ) const ;
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
* get_latitude_diff - returns the current latitude difference from the home location .
*
* @ return difference in 100 nano degrees ( i . e . degree value multiplied by 10 , 000 , 000 )
*/
2014-01-03 20:16:07 -04:00
virtual float get_latitude_diff ( ) const ;
2013-10-25 10:55:31 -03:00
/**
* get_longitude_diff - returns the current longitude difference from the home location .
*
* @ return difference in 100 nano degrees ( i . e . degree value multiplied by 10 , 000 , 000 )
*/
2014-01-03 20:16:07 -04:00
virtual float get_longitude_diff ( ) const ;
2013-03-27 02:09:04 -03:00
2013-10-25 10:55:31 -03:00
/**
* get_velocity - returns the current velocity in cm / s
*
* @ return velocity vector :
* . x : latitude velocity in cm / s
* . y : longitude velocity in cm / s
* . z : vertical velocity in cm / s
*/
2014-01-03 20:16:07 -04:00
virtual const Vector3f & get_velocity ( ) const { return _velocity ; }
2013-10-25 10:55:31 -03:00
2013-10-29 01:25:14 -03:00
/**
* get_velocity_xy - returns the current horizontal velocity in cm / s
*
* @ returns the current horizontal velocity in cm / s
*/
2014-04-21 09:58:42 -03:00
virtual float get_velocity_xy ( ) const ;
2013-10-29 01:25:14 -03:00
2013-10-25 10:55:31 -03:00
/**
* set_velocity_xy - overwrites the current horizontal velocity in cm / s
*
* @ param x : latitude velocity in cm / s
* @ param y : longitude velocity in cm / s
*/
2013-03-19 05:51:16 -03:00
void set_velocity_xy ( float x , float y ) ;
2012-11-05 00:31:02 -04:00
2014-01-03 07:41:03 -04:00
/**
setup_home_position - reset on home position change
*/
void setup_home_position ( void ) ;
2012-12-09 11:43:11 -04:00
//
// Z Axis methods
//
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
* altitude_ok - returns true if inertial based altitude and position can be trusted
* @ return
*/
2014-01-03 20:16:07 -04:00
virtual bool altitude_ok ( ) const { return true ; }
2012-11-05 00:31:02 -04:00
2013-10-25 10:55:31 -03:00
/**
2014-01-03 20:16:07 -04:00
* get_altitude - get latest altitude estimate in cm above the
* reference position
2013-10-25 10:55:31 -03:00
* @ return
*/
2014-01-03 20:16:07 -04:00
virtual float get_altitude ( ) const { return _position . z ; }
2013-10-25 10:55:31 -03:00
/**
* set_altitude - overwrites the current altitude value .
*
* @ param new_altitude : altitude in cm
*/
2013-03-19 05:51:16 -03:00
void set_altitude ( float new_altitude ) ;
2012-12-09 11:43:11 -04:00
2013-10-25 10:55:31 -03:00
/**
* get_velocity_z - returns the current climbrate .
*
* @ see get_velocity ( ) . z
*
2014-01-03 20:16:07 -04:00
* @ return climbrate in cm / s ( positive up )
2013-10-25 10:55:31 -03:00
*/
2014-01-03 20:16:07 -04:00
virtual float get_velocity_z ( ) const { return _velocity . z ; }
2013-10-25 10:55:31 -03:00
/**
* set_velocity_z - overwrites the current climbrate .
*
* @ param new_velocity : climbrate in cm / s
*/
2013-03-19 05:51:16 -03:00
void set_velocity_z ( float new_velocity ) ;
2012-12-09 11:43:11 -04:00
2013-11-21 02:32:16 -04:00
/**
* error_count - returns number of missed updates from GPS
*/
2014-07-14 22:59:40 -03:00
uint8_t error_count ( ) const { return _error_count ; }
2013-11-21 02:32:16 -04:00
/**
* ignore_next_error - the next error ( if it occurs immediately ) will not be added to the error count
*/
void ignore_next_error ( ) { _flags . ignore_error = 7 ; }
2012-12-09 11:43:11 -04:00
// class level parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
// public variables
2014-06-19 20:53:12 -03:00
Vector3f accel_correction_hbf ; // horizontal body frame accelerometer corrections. here for logging purposes only
2012-12-09 11:43:11 -04:00
protected :
2014-01-05 01:35:08 -04:00
/**
* correct_with_gps - calculates horizontal position error using gps
*
* @ param now : current time since boot in milliseconds
* @ param lon : longitude in 100 nano degrees ( i . e . degree value multiplied by 10 , 000 , 000 )
* @ param lat : latitude in 100 nano degrees ( i . e . degree value multiplied by 10 , 000 , 000 )
*/
void correct_with_gps ( uint32_t now , int32_t lon , int32_t lat ) ;
/**
* check_gps - checks if new gps readings have arrived and calls correct_with_gps to
* calculate the horizontal position error
* @ see correct_with_gps ( int32_t lon , int32_t lat , float dt ) ;
*/
void check_gps ( ) ;
/**
* check_baro - checks if new baro readings have arrived and calls correct_with_baro to
* calculate the vertical position error
*
* @ see correct_with_baro ( float baro_alt , float dt ) ;
*/
void check_baro ( ) ;
/**
* correct_with_baro - calculates vertical position error using barometer .
*
* @ param baro_alt : altitude in cm
* @ param dt : time since last baro reading in s
*/
void correct_with_baro ( float baro_alt , float dt ) ;
2013-10-25 10:55:31 -03:00
/**
* update gains from time constant .
*/
void update_gains ( ) ;
/**
* set_position_xy - overwrites the current position relative to the home location in cm
*
* the home location was set with AP_InertialNav : : set_home_location ( int32_t , int32_t )
*
* @ param x : relative latitude position in cm
* @ param y : relative longitude position in cm
*/
void set_position_xy ( float x , float y ) ;
2012-12-09 11:43:11 -04:00
2013-09-19 03:56:23 -03:00
// structure for holding flags
struct InertialNav_flags {
2013-11-21 02:32:16 -04:00
uint8_t gps_glitching : 1 ; // 1 if glitch detector was previously indicating a gps glitch
2014-07-28 05:36:46 -03:00
uint8_t baro_glitching : 1 ; // 1 if baro glitch detector was previously indicating a baro glitch
2013-11-21 02:32:16 -04:00
uint8_t ignore_error : 3 ; // the number of iterations for which we should ignore errors
2013-09-19 03:56:23 -03:00
} _flags ;
2014-01-03 20:16:07 -04:00
AP_AHRS & _ahrs ; // reference to ahrs object
2014-01-02 01:04:52 -04:00
AP_Baro & _baro ; // reference to barometer
2012-12-09 11:43:11 -04:00
2014-07-14 22:54:59 -03:00
// parameters
AP_Float _time_constant_xy ; // time constant for horizontal corrections in s
AP_Float _time_constant_z ; // time constant for vertical corrections in s
2012-12-09 11:43:11 -04:00
// XY Axis specific variables
bool _xy_enabled ; // xy position estimates enabled
float _k1_xy ; // gain for horizontal position correction
float _k2_xy ; // gain for horizontal velocity correction
float _k3_xy ; // gain for horizontal accelerometer offset correction
2013-10-25 10:55:31 -03:00
uint32_t _gps_last_update ; // system time of last gps update in ms
uint32_t _gps_last_time ; // time of last gps update according to the gps itself in ms
2012-12-09 11:43:11 -04:00
uint8_t _historic_xy_counter ; // counter used to slow saving of position estimates for later comparison to gps
2013-10-25 10:55:31 -03:00
AP_BufferFloat_Size5 _hist_position_estimate_x ; // buffer of historic accel based position to account for gpslag
AP_BufferFloat_Size5 _hist_position_estimate_y ; // buffer of historic accel based position to account for gps lag
2013-08-07 11:00:09 -03:00
float _lon_to_cm_scaling ; // conversion of longitude to centimeters
2013-10-29 01:25:14 -03:00
2012-12-09 11:43:11 -04:00
// Z Axis specific variables
float _k1_z ; // gain for vertical position correction
float _k2_z ; // gain for vertical velocity correction
float _k3_z ; // gain for vertical accelerometer offset correction
2013-10-25 10:55:31 -03:00
uint32_t _baro_last_update ; // time of last barometer update in ms
AP_BufferFloat_Size15 _hist_position_estimate_z ; // buffer of historic accel based altitudes to account for barometer lag
2012-12-09 11:43:11 -04:00
// general variables
2013-10-25 10:55:31 -03:00
Vector3f _position_base ; // (uncorrected) position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
Vector3f _position_correction ; // sum of corrections to _position_base from delayed 1st order samples in cm
Vector3f _velocity ; // latest velocity estimate (integrated from accelerometer values) in cm/s
Vector3f _position_error ; // current position error in cm - is set by the check_* methods and used by update method to calculate the correction terms
Vector3f _position ; // sum(_position_base, _position_correction) - corrected position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
2013-09-19 03:56:23 -03:00
2013-11-21 02:32:16 -04:00
// error handling
GPS_Glitch & _glitch_detector ; // GPS Glitch detector
2014-07-28 05:36:46 -03:00
Baro_Glitch & _baro_glitch ; // Baro glitch detector
2013-11-21 02:32:16 -04:00
uint8_t _error_count ; // number of missed GPS updates
2012-11-05 00:31:02 -04:00
} ;
2014-01-03 07:57:50 -04:00
# if AP_AHRS_NAVEKF_AVAILABLE
# include "AP_InertialNav_NavEKF.h"
# endif
2012-11-07 09:24:00 -04:00
# endif // __AP_INERTIALNAV_H__