2016-02-17 21:25:13 -04:00
# pragma once
2014-01-01 21:15:58 -04:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
/*
* NavEKF based AHRS ( Attitude Heading Reference System ) interface for
* ArduPilot
*
*/
2015-08-11 03:28:42 -03:00
# include <AP_HAL/AP_HAL.h>
2018-04-10 05:56:06 -03:00
# define AP_AHRS_NAVEKF_AVAILABLE 1
2015-08-11 03:28:42 -03:00
# include "AP_AHRS.h"
2014-01-01 21:15:58 -04:00
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
# include <SITL/SITL.h>
# endif
2015-09-22 21:35:02 -03:00
# include <AP_NavEKF2/AP_NavEKF2.h>
2016-07-14 02:08:42 -03:00
# include <AP_NavEKF3/AP_NavEKF3.h>
2015-10-12 06:50:01 -03:00
# include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
2014-01-01 21:15:58 -04:00
2018-04-10 05:56:06 -03:00
2014-10-02 01:43:46 -03:00
# define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
2014-01-01 21:15:58 -04:00
2017-08-30 04:59:36 -03:00
class AP_AHRS_NavEKF : public AP_AHRS_DCM {
2014-01-01 21:15:58 -04:00
public :
2015-10-13 15:08:49 -03:00
enum Flags {
FLAG_NONE = 0 ,
FLAG_ALWAYS_USE_EKF = 0x1 ,
} ;
2017-12-12 21:06:11 -04:00
// Constructor
2018-03-10 05:35:03 -04:00
AP_AHRS_NavEKF ( NavEKF2 & _EKF2 , NavEKF3 & _EKF3 , Flags flags = FLAG_NONE ) ;
2017-08-30 04:59:36 -03:00
/* Do not allow copies */
AP_AHRS_NavEKF ( const AP_AHRS_NavEKF & other ) = delete ;
AP_AHRS_NavEKF & operator = ( const AP_AHRS_NavEKF & ) = delete ;
2014-01-01 21:15:58 -04:00
// return the smoothed gyro vector corrected for drift
2016-09-03 04:12:39 -03:00
const Vector3f & get_gyro ( void ) const override ;
const Matrix3f & get_rotation_body_to_ned ( void ) const override ;
2014-01-01 21:15:58 -04:00
// return the current drift correction integrator value
2016-09-03 04:12:39 -03:00
const Vector3f & get_gyro_drift ( void ) const override ;
2014-01-01 21:15:58 -04:00
2014-10-28 08:22:48 -03:00
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
2017-02-23 06:27:21 -04:00
void reset_gyro_drift ( ) override ;
2014-10-28 08:22:48 -03:00
2017-04-02 22:03:45 -03:00
void update ( bool skip_ins_update = false ) override ;
2017-02-23 06:27:21 -04:00
void reset ( bool recover_eulers = false ) override ;
2014-01-01 21:15:58 -04:00
// reset the current attitude, used on new IMU calibration
2017-02-23 06:27:21 -04:00
void reset_attitude ( const float & roll , const float & pitch , const float & yaw ) override ;
2014-01-01 21:15:58 -04:00
// dead-reckoning support
2017-02-23 06:27:21 -04:00
bool get_position ( struct Location & loc ) const override ;
2014-01-01 21:15:58 -04:00
2017-01-05 14:07:14 -04:00
// get latest altitude estimate above ground level in meters and validity flag
2018-01-05 21:25:04 -04:00
bool get_hagl ( float & hagl ) const override ;
2015-10-12 06:50:01 -03:00
2014-01-01 21:15:58 -04:00
// status reporting of estimated error
2017-02-23 06:27:21 -04:00
float get_error_rp ( ) const override ;
float get_error_yaw ( ) const override ;
2014-01-01 21:15:58 -04:00
// return a wind estimation vector, in m/s
2017-12-02 09:13:32 -04:00
Vector3f wind_estimate ( ) const override ;
2014-01-01 21:15:58 -04:00
// return an airspeed estimate if available. return true
// if we have an estimate
2017-02-23 06:27:21 -04:00
bool airspeed_estimate ( float * airspeed_ret ) const override ;
2014-01-01 21:15:58 -04:00
// true if compass is being used
2017-02-23 06:27:21 -04:00
bool use_compass ( ) override ;
2014-01-01 21:15:58 -04:00
2015-09-22 22:40:25 -03:00
// we will need to remove these to fully hide which EKF we are using
2015-09-23 04:29:43 -03:00
NavEKF2 & get_NavEKF2 ( void ) {
return EKF2 ;
}
const NavEKF2 & get_NavEKF2_const ( void ) const {
return EKF2 ;
}
2014-01-01 21:15:58 -04:00
2016-07-14 02:08:42 -03:00
NavEKF3 & get_NavEKF3 ( void ) {
return EKF3 ;
}
const NavEKF3 & get_NavEKF3_const ( void ) const {
return EKF3 ;
}
2019-02-19 23:53:28 -04:00
2014-01-01 22:47:40 -04:00
// return secondary attitude solution if available, as eulers in radians
2017-12-02 09:13:32 -04:00
bool get_secondary_attitude ( Vector3f & eulers ) const override ;
2014-01-01 22:47:40 -04:00
2017-04-15 08:21:09 -03:00
// return secondary attitude solution if available, as quaternion
2017-12-02 09:13:32 -04:00
bool get_secondary_quaternion ( Quaternion & quat ) const override ;
2019-02-19 23:53:28 -04:00
2014-01-01 22:47:40 -04:00
// return secondary position solution if available
2017-12-02 09:13:32 -04:00
bool get_secondary_position ( struct Location & loc ) const override ;
2014-01-01 22:47:40 -04:00
2014-01-01 23:25:41 -04:00
// EKF has a better ground speed vector estimate
2017-02-23 06:27:21 -04:00
Vector2f groundspeed_vector ( ) override ;
2014-01-01 23:25:41 -04:00
2016-09-03 04:12:39 -03:00
const Vector3f & get_accel_ef ( uint8_t i ) const override ;
const Vector3f & get_accel_ef ( ) const override ;
2014-10-31 19:07:18 -03:00
2016-08-11 16:58:02 -03:00
// Retrieves the corrected NED delta velocity in use by the inertial navigation
2017-02-23 06:27:21 -04:00
void getCorrectedDeltaVelocityNED ( Vector3f & ret , float & dt ) const override ;
2016-08-11 16:58:02 -03:00
2014-10-31 19:07:18 -03:00
// blended accelerometer values in the earth frame in m/s/s
2017-02-23 06:27:21 -04:00
const Vector3f & get_accel_ef_blended ( ) const override ;
2014-10-31 19:07:18 -03:00
2017-04-19 03:28:14 -03:00
// set the EKF's origin location in 10e7 degrees. This should only
// be called when the EKF has no absolute position reference (i.e. GPS)
// from which to decide the origin on its own
bool set_origin ( const Location & loc ) override ;
2015-10-12 06:50:01 -03:00
// returns the inertial navigation origin in lat/lon/alt
2017-11-30 00:35:10 -04:00
bool get_origin ( Location & ret ) const override ;
2015-10-12 06:50:01 -03:00
2017-02-23 06:27:21 -04:00
bool have_inertial_nav ( ) const override ;
2014-01-03 20:15:34 -04:00
2017-02-23 06:27:21 -04:00
bool get_velocity_NED ( Vector3f & vec ) const override ;
2014-01-03 20:15:34 -04:00
2017-01-30 15:06:13 -04:00
// return the relative position NED to either home or origin
2016-07-09 08:36:09 -03:00
// return true if the estimate is valid
2017-02-23 06:26:42 -04:00
bool get_relative_position_NED_home ( Vector3f & vec ) const override ;
bool get_relative_position_NED_origin ( Vector3f & vec ) const override ;
2016-07-09 08:36:09 -03:00
2017-01-30 15:06:13 -04:00
// return the relative position NE to either home or origin
2016-07-09 08:36:09 -03:00
// return true if the estimate is valid
2017-02-23 06:26:42 -04:00
bool get_relative_position_NE_home ( Vector2f & posNE ) const override ;
bool get_relative_position_NE_origin ( Vector2f & posNE ) const override ;
2017-01-30 15:06:13 -04:00
// return the relative position down to either home or origin
// baro will be used for the _home relative one if the EKF isn't
2017-02-23 06:26:42 -04:00
void get_relative_position_D_home ( float & posD ) const override ;
bool get_relative_position_D_origin ( float & posD ) const override ;
2016-07-09 08:36:09 -03:00
2015-10-12 06:50:01 -03:00
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
2017-01-05 14:07:14 -04:00
// This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
2017-12-02 09:13:32 -04:00
bool get_vert_pos_rate ( float & velocity ) const ;
2015-10-12 06:50:01 -03:00
2014-08-06 21:31:24 -03:00
// write optical flow measurements to EKF
2018-09-02 08:24:14 -03:00
void writeOptFlowMeas ( const uint8_t rawFlowQuality , const Vector2f & rawFlowRates , const Vector2f & rawGyroRates , const uint32_t msecFlowMeas , const Vector3f & posOffset ) ;
2017-03-21 18:13:54 -03:00
// write body odometry measurements to the EKF
void writeBodyFrameOdom ( float quality , const Vector3f & delPos , const Vector3f & delAng , float delTime , uint32_t timeStamp_ms , const Vector3f & posOffset ) ;
2014-08-06 21:31:24 -03:00
2018-03-07 20:44:53 -04:00
// Write position and quaternion data from an external navigation system
void writeExtNavData ( const Vector3f & sensOffset , const Vector3f & pos , const Quaternion & quat , float posErr , float angErr , uint32_t timeStamp_ms , uint32_t resetTime_ms ) override ;
2017-01-05 14:07:14 -04:00
// inhibit GPS usage
2014-11-12 14:49:15 -04:00
uint8_t setInhibitGPS ( void ) ;
2014-11-15 19:36:33 -04:00
// get speed limit
2017-12-02 09:13:32 -04:00
void getEkfControlLimits ( float & ekfGndSpdLimit , float & ekfNavVelGainScaler ) const ;
2014-11-15 19:36:33 -04:00
2015-03-19 02:58:13 -03:00
void set_ekf_use ( bool setting ) ;
2014-01-03 21:39:20 -04:00
2014-05-15 04:09:18 -03:00
// is the AHRS subsystem healthy?
2017-02-23 06:27:21 -04:00
bool healthy ( ) const override ;
2019-03-23 15:21:47 -03:00
bool prearm_healthy ( ) const override ;
2014-05-15 04:09:18 -03:00
2014-10-02 01:43:46 -03:00
// true if the AHRS has completed initialisation
2017-02-23 06:27:21 -04:00
bool initialised ( ) const override ;
2014-09-29 05:37:14 -03:00
2015-10-12 06:50:01 -03:00
// get_filter_status - returns filter status as a series of flags
bool get_filter_status ( nav_filter_status & status ) const ;
2015-03-16 18:29:11 -03:00
// get compass offset estimates
// true if offsets are valid
2017-12-02 09:13:32 -04:00
bool getMagOffsets ( uint8_t mag_idx , Vector3f & magOffsets ) const ;
2015-03-16 18:29:11 -03:00
2015-09-08 02:50:22 -03:00
// report any reason for why the backend is refusing to initialise
const char * prearm_failure_reason ( void ) const override ;
2019-02-19 23:52:55 -04:00
// check all cores providing consistent attitudes for prearm checks
2019-03-06 02:50:11 -04:00
bool attitudes_consistent ( char * failure_msg , const uint8_t failure_msg_len ) const override ;
2019-02-19 23:52:55 -04:00
2015-09-23 04:46:51 -03:00
// return the amount of yaw angle change due to the last yaw angle reset in radians
2015-09-24 03:51:21 -03:00
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
2017-02-23 06:27:21 -04:00
uint32_t getLastYawResetAngle ( float & yawAng ) const override ;
2015-09-23 04:46:51 -03:00
2017-01-05 14:07:14 -04:00
// return the amount of NE position change in meters due to the last reset
2015-10-29 02:18:04 -03:00
// returns the time of the last reset or 0 if no reset has ever occurred
2017-02-23 06:27:21 -04:00
uint32_t getLastPosNorthEastReset ( Vector2f & pos ) const override ;
2015-10-29 02:18:04 -03:00
2017-01-05 14:07:14 -04:00
// return the amount of NE velocity change in meters/sec due to the last reset
2015-10-29 02:18:04 -03:00
// returns the time of the last reset or 0 if no reset has ever occurred
2017-02-23 06:27:21 -04:00
uint32_t getLastVelNorthEastReset ( Vector2f & vel ) const override ;
2015-10-29 02:18:04 -03:00
2016-11-22 01:28:14 -04:00
// return the amount of vertical position change due to the last reset in meters
// returns the time of the last reset or 0 if no reset has ever occurred
2017-02-23 06:27:21 -04:00
uint32_t getLastPosDownReset ( float & posDelta ) const override ;
2016-11-22 01:28:14 -04:00
2015-09-23 04:46:51 -03:00
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
2017-02-23 06:27:21 -04:00
bool resetHeightDatum ( ) override ;
2015-09-28 21:58:54 -03:00
// send a EKF_STATUS_REPORT for current EKF
2017-12-02 09:13:32 -04:00
void send_ekf_status_report ( mavlink_channel_t chan ) const ;
2019-02-19 23:53:28 -04:00
2017-01-05 14:07:14 -04:00
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag
2015-10-12 06:50:01 -03:00
// this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required
2017-12-07 00:51:28 -04:00
bool get_hgt_ctrl_limit ( float & limit ) const override ;
2015-10-12 06:50:01 -03:00
2017-12-07 19:26:28 -04:00
// get_location - updates the provided location with the latest
// calculated location including absolute altitude
// returns true on success (i.e. the EKF knows it's latest
// position), false on failure
2015-10-12 06:50:01 -03:00
bool get_location ( struct Location & loc ) const ;
2015-10-17 02:49:50 -03:00
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
2017-01-05 14:07:14 -04:00
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter
2015-10-17 02:49:50 -03:00
// boolean false is returned if variances are not available
2016-09-21 18:00:21 -03:00
bool get_variances ( float & velVar , float & posVar , float & hgtVar , Vector3f & magVar , float & tasVar , Vector2f & offset ) const override ;
2015-10-17 02:49:50 -03:00
2016-01-04 19:57:17 -04:00
// returns the expected NED magnetic field
bool get_mag_field_NED ( Vector3f & ret ) const ;
// returns the estimated magnetic field offsets in body frame
2017-02-23 06:27:21 -04:00
bool get_mag_field_correction ( Vector3f & ret ) const override ;
2016-01-04 19:57:17 -04:00
2015-12-22 15:18:03 -04:00
void setTakeoffExpected ( bool val ) ;
void setTouchdownExpected ( bool val ) ;
2017-12-02 09:13:32 -04:00
bool getGpsGlitchStatus ( ) const ;
2016-01-26 17:28:21 -04:00
2016-05-03 03:34:05 -03:00
// used by Replay to force start at right timestamp
2016-12-19 00:38:44 -04:00
void force_ekf_start ( void ) { _force_ekf = true ; }
2016-05-04 01:02:12 -03:00
// is the EKF backend doing its own sensor logging?
bool have_ekf_logging ( void ) const override ;
2016-09-03 04:54:36 -03:00
// get the index of the current primary accelerometer sensor
uint8_t get_primary_accel_index ( void ) const override ;
// get the index of the current primary gyro sensor
uint8_t get_primary_gyro_index ( void ) const override ;
2017-04-09 08:17:05 -03:00
2014-01-01 21:15:58 -04:00
private :
2015-11-20 04:34:30 -04:00
enum EKF_TYPE { EKF_TYPE_NONE = 0 ,
2016-06-28 04:36:12 -03:00
EKF_TYPE3 = 3 ,
EKF_TYPE2 = 2
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
, EKF_TYPE_SITL = 10
# endif
} ;
2015-09-23 04:53:44 -03:00
EKF_TYPE active_EKF_type ( void ) const ;
2014-01-02 01:16:29 -04:00
2015-10-13 15:08:49 -03:00
bool always_use_EKF ( ) const {
2016-03-04 17:52:40 -04:00
return _ekf_flags & FLAG_ALWAYS_USE_EKF ;
2015-10-13 15:08:49 -03:00
}
2015-09-22 21:35:02 -03:00
NavEKF2 & EKF2 ;
2016-07-14 02:08:42 -03:00
NavEKF3 & EKF3 ;
2016-12-19 00:38:44 -04:00
bool _ekf2_started ;
bool _ekf3_started ;
bool _force_ekf ;
2019-04-03 12:40:37 -03:00
// rotation from vehicle body to NED frame
2014-01-01 22:05:28 -04:00
Matrix3f _dcm_matrix ;
2014-01-01 22:47:40 -04:00
Vector3f _dcm_attitude ;
2019-04-03 12:40:37 -03:00
2017-02-08 02:35:33 -04:00
Vector3f _gyro_drift ;
2014-07-13 08:56:39 -03:00
Vector3f _gyro_estimate ;
2014-10-31 19:07:18 -03:00
Vector3f _accel_ef_ekf [ INS_MAX_INSTANCES ] ;
Vector3f _accel_ef_ekf_blended ;
2015-09-22 22:40:25 -03:00
const uint16_t startup_delay_ms = 1000 ;
uint32_t start_time_ms = 0 ;
2016-03-04 17:52:40 -04:00
Flags _ekf_flags ;
2015-09-22 22:40:25 -03:00
uint8_t ekf_type ( void ) const ;
2017-04-02 22:03:45 -03:00
void update_DCM ( bool skip_ins_update ) ;
2015-09-22 22:40:25 -03:00
void update_EKF2 ( void ) ;
2016-07-14 02:08:42 -03:00
void update_EKF3 ( void ) ;
2015-11-20 04:34:30 -04:00
2016-09-03 04:54:36 -03:00
// get the index of the current primary IMU
uint8_t get_primary_IMU_index ( void ) const ;
2019-02-19 23:53:28 -04:00
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL : : SITL * _sitl ;
2017-03-21 19:38:50 -03:00
uint32_t _last_body_odm_update_ms = 0 ;
2015-11-20 04:34:30 -04:00
void update_SITL ( void ) ;
# endif
2014-01-01 21:15:58 -04:00
} ;