2015-09-23 04:29:43 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
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>
# include "AP_AHRS.h"
# include <AP_Vehicle/AP_Vehicle.h>
2015-09-08 02:50:22 -03:00
# include <GCS_MAVLink/GCS.h>
2016-07-07 05:26:53 -03:00
# include <AP_Module/AP_Module.h>
2014-01-01 21:15:58 -04:00
# if AP_AHRS_NAVEKF_AVAILABLE
2014-01-01 22:05:28 -04:00
extern const AP_HAL : : HAL & hal ;
2015-11-20 04:34:30 -04:00
// constructor
AP_AHRS_NavEKF : : AP_AHRS_NavEKF ( AP_InertialSensor & ins , AP_Baro & baro , AP_GPS & gps , RangeFinder & rng ,
NavEKF & _EKF1 , NavEKF2 & _EKF2 , Flags flags ) :
AP_AHRS_DCM ( ins , baro , gps ) ,
EKF1 ( _EKF1 ) ,
EKF2 ( _EKF2 ) ,
2016-03-04 17:52:40 -04:00
_ekf_flags ( flags )
2015-11-20 04:34:30 -04:00
{
2015-12-01 00:19:45 -04:00
_dcm_matrix . identity ( ) ;
2015-11-20 04:34:30 -04:00
}
2014-01-01 21:15:58 -04:00
// return the smoothed gyro vector corrected for drift
2014-07-13 08:56:39 -03:00
const Vector3f & AP_AHRS_NavEKF : : get_gyro ( void ) const
2014-01-01 21:15:58 -04:00
{
2015-09-23 04:53:44 -03:00
if ( ! active_EKF_type ( ) ) {
2014-04-15 17:21:12 -03:00
return AP_AHRS_DCM : : get_gyro ( ) ;
}
2014-07-13 08:56:39 -03:00
return _gyro_estimate ;
2014-01-01 21:15:58 -04:00
}
2015-12-10 18:05:13 -04:00
const Matrix3f & AP_AHRS_NavEKF : : get_rotation_body_to_ned ( void ) const
2014-01-01 21:15:58 -04:00
{
2015-09-23 04:53:44 -03:00
if ( ! active_EKF_type ( ) ) {
2015-12-10 18:05:13 -04:00
return AP_AHRS_DCM : : get_rotation_body_to_ned ( ) ;
2014-01-01 22:05:28 -04:00
}
return _dcm_matrix ;
2014-01-01 21:15:58 -04:00
}
const Vector3f & AP_AHRS_NavEKF : : get_gyro_drift ( void ) const
{
2015-09-23 04:53:44 -03:00
if ( ! active_EKF_type ( ) ) {
2014-07-13 08:56:39 -03:00
return AP_AHRS_DCM : : get_gyro_drift ( ) ;
}
return _gyro_bias ;
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
void AP_AHRS_NavEKF : : reset_gyro_drift ( void )
{
// update DCM
AP_AHRS_DCM : : reset_gyro_drift ( ) ;
2014-10-28 22:20:43 -03:00
// reset the EKF gyro bias states
2015-09-22 22:40:25 -03:00
EKF1 . resetGyroBias ( ) ;
EKF2 . resetGyroBias ( ) ;
2014-10-28 08:22:48 -03:00
}
2014-01-01 21:15:58 -04:00
void AP_AHRS_NavEKF : : update ( void )
2015-09-22 22:40:25 -03:00
{
2016-04-03 22:08:03 -03:00
# if !AP_AHRS_WITH_EKF1
if ( _ekf_type = = 1 ) {
_ekf_type . set ( 2 ) ;
}
# endif
2015-09-22 22:40:25 -03:00
update_DCM ( ) ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
update_EKF1 ( ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
update_EKF2 ( ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
update_SITL ( ) ;
# endif
2016-07-07 05:26:53 -03:00
// call AHRS_update hook if any
AP_Module : : call_hook_AHRS_update ( * this ) ;
2015-09-22 22:40:25 -03:00
}
void AP_AHRS_NavEKF : : update_DCM ( void )
2014-01-01 21:15:58 -04:00
{
2014-10-14 21:15:33 -03:00
// we need to restore the old DCM attitude values as these are
// used internally in DCM to calculate error values for gyro drift
// correction
roll = _dcm_attitude . x ;
pitch = _dcm_attitude . y ;
yaw = _dcm_attitude . z ;
2014-10-14 23:18:08 -03:00
update_cd_values ( ) ;
2014-10-14 21:15:33 -03:00
2014-01-01 21:15:58 -04:00
AP_AHRS_DCM : : update ( ) ;
2014-01-01 22:47:40 -04:00
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude ( roll , pitch , yaw ) ;
2015-09-22 22:40:25 -03:00
}
void AP_AHRS_NavEKF : : update_EKF1 ( void )
{
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
if ( ! ekf1_started ) {
// wait 1 second for DCM to output a valid tilt error estimate
if ( start_time_ms = = 0 ) {
2015-11-19 23:06:30 -04:00
start_time_ms = AP_HAL : : millis ( ) ;
2015-09-22 22:40:25 -03:00
}
2016-01-06 17:34:32 -04:00
// slight extra delay on EKF1 to prioritise EKF2 for memory
2016-05-03 03:34:05 -03:00
if ( AP_HAL : : millis ( ) - start_time_ms > startup_delay_ms + 100U | | force_ekf ) {
2015-09-22 22:40:25 -03:00
ekf1_started = EKF1 . InitialiseFilterDynamic ( ) ;
2016-05-03 03:34:05 -03:00
if ( force_ekf ) {
return ;
}
2015-09-22 22:40:25 -03:00
}
}
if ( ekf1_started ) {
EKF1 . UpdateFilter ( ) ;
2015-09-23 04:53:44 -03:00
if ( active_EKF_type ( ) = = EKF_TYPE1 ) {
2015-09-22 22:40:25 -03:00
Vector3f eulers ;
2015-11-20 03:34:00 -04:00
EKF1 . getRotationBodyToNED ( _dcm_matrix ) ;
2015-09-22 22:40:25 -03:00
EKF1 . getEulerAngles ( eulers ) ;
roll = eulers . x ;
pitch = eulers . y ;
yaw = eulers . z ;
update_cd_values ( ) ;
update_trig ( ) ;
// keep _gyro_bias for get_gyro_drift()
EKF1 . getGyroBias ( _gyro_bias ) ;
_gyro_bias = - _gyro_bias ;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate . zero ( ) ;
2015-09-23 04:29:43 -03:00
uint8_t healthy_count = 0 ;
2015-09-22 22:40:25 -03:00
for ( uint8_t i = 0 ; i < _ins . get_gyro_count ( ) ; i + + ) {
2016-05-01 09:16:06 -03:00
if ( _ins . get_gyro_health ( i ) & & healthy_count < 2 & & _ins . use_gyro ( i ) ) {
2015-09-22 22:40:25 -03:00
_gyro_estimate + = _ins . get_gyro ( i ) ;
healthy_count + + ;
}
}
if ( healthy_count > 1 ) {
_gyro_estimate / = healthy_count ;
}
_gyro_estimate + = _gyro_bias ;
2014-01-01 22:47:40 -04:00
2015-09-22 22:40:25 -03:00
float abias1 , abias2 ;
EKF1 . getAccelZBias ( abias1 , abias2 ) ;
// update _accel_ef_ekf
for ( uint8_t i = 0 ; i < _ins . get_accel_count ( ) ; i + + ) {
Vector3f accel = _ins . get_accel ( i ) ;
if ( i = = 0 ) {
accel . z - = abias1 ;
} else if ( i = = 1 ) {
accel . z - = abias2 ;
}
if ( _ins . get_accel_health ( i ) ) {
_accel_ef_ekf [ i ] = _dcm_matrix * accel ;
}
}
if ( _ins . use_accel ( 0 ) & & _ins . use_accel ( 1 ) ) {
float IMU1_weighting ;
EKF1 . getIMU1Weighting ( IMU1_weighting ) ;
_accel_ef_ekf_blended = _accel_ef_ekf [ 0 ] * IMU1_weighting + _accel_ef_ekf [ 1 ] * ( 1.0f - IMU1_weighting ) ;
} else {
_accel_ef_ekf_blended = _accel_ef_ekf [ _ins . get_primary_accel ( ) ] ;
}
}
}
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
}
void AP_AHRS_NavEKF : : update_EKF2 ( void )
{
if ( ! ekf2_started ) {
2015-04-21 22:21:44 -03:00
// wait 1 second for DCM to output a valid tilt error estimate
2014-10-28 21:55:53 -03:00
if ( start_time_ms = = 0 ) {
2015-11-19 23:06:30 -04:00
start_time_ms = AP_HAL : : millis ( ) ;
2014-10-28 21:55:53 -03:00
}
2016-05-03 03:34:05 -03:00
if ( AP_HAL : : millis ( ) - start_time_ms > startup_delay_ms | | force_ekf ) {
2015-09-22 22:40:25 -03:00
ekf2_started = EKF2 . InitialiseFilter ( ) ;
2016-05-03 03:34:05 -03:00
if ( force_ekf ) {
return ;
}
2014-01-01 22:05:28 -04:00
}
}
2015-09-22 22:40:25 -03:00
if ( ekf2_started ) {
EKF2 . UpdateFilter ( ) ;
2015-09-23 04:53:44 -03:00
if ( active_EKF_type ( ) = = EKF_TYPE2 ) {
2014-01-01 22:05:28 -04:00
Vector3f eulers ;
2015-11-20 03:34:00 -04:00
EKF2 . getRotationBodyToNED ( _dcm_matrix ) ;
2015-11-07 08:03:41 -04:00
EKF2 . getEulerAngles ( - 1 , eulers ) ;
2014-01-01 22:05:28 -04:00
roll = eulers . x ;
pitch = eulers . y ;
yaw = eulers . z ;
2014-10-14 23:18:08 -03:00
update_cd_values ( ) ;
2014-02-13 04:53:14 -04:00
update_trig ( ) ;
2014-07-13 08:56:39 -03:00
// keep _gyro_bias for get_gyro_drift()
2015-11-07 08:03:41 -04:00
EKF2 . getGyroBias ( - 1 , _gyro_bias ) ;
2014-07-13 08:56:39 -03:00
_gyro_bias = - _gyro_bias ;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate . zero ( ) ;
2015-09-23 04:29:43 -03:00
uint8_t healthy_count = 0 ;
2014-07-13 08:56:39 -03:00
for ( uint8_t i = 0 ; i < _ins . get_gyro_count ( ) ; i + + ) {
2016-05-01 09:16:06 -03:00
if ( _ins . get_gyro_health ( i ) & & healthy_count < 2 & & _ins . use_gyro ( i ) ) {
2014-07-13 08:56:39 -03:00
_gyro_estimate + = _ins . get_gyro ( i ) ;
healthy_count + + ;
}
}
if ( healthy_count > 1 ) {
_gyro_estimate / = healthy_count ;
}
_gyro_estimate + = _gyro_bias ;
2014-10-31 19:07:18 -03:00
2015-09-23 22:10:11 -03:00
float abias ;
2015-11-07 08:03:41 -04:00
EKF2 . getAccelZBias ( - 1 , abias ) ;
2015-03-28 04:08:11 -03:00
2015-09-23 22:10:11 -03:00
// This EKF uses the primary IMU
// Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
2014-10-31 19:07:18 -03:00
// update _accel_ef_ekf
for ( uint8_t i = 0 ; i < _ins . get_accel_count ( ) ; i + + ) {
2015-03-28 04:08:11 -03:00
Vector3f accel = _ins . get_accel ( i ) ;
2015-09-23 22:10:11 -03:00
if ( i = = _ins . get_primary_accel ( ) ) {
accel . z - = abias ;
2015-03-28 04:08:11 -03:00
}
2014-10-31 19:07:18 -03:00
if ( _ins . get_accel_health ( i ) ) {
2015-03-28 04:08:11 -03:00
_accel_ef_ekf [ i ] = _dcm_matrix * accel ;
2014-10-31 19:07:18 -03:00
}
}
2015-09-23 22:10:11 -03:00
_accel_ef_ekf_blended = _accel_ef_ekf [ _ins . get_primary_accel ( ) ] ;
2014-01-01 22:05:28 -04:00
}
}
2014-01-01 21:15:58 -04:00
}
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
void AP_AHRS_NavEKF : : update_SITL ( void )
{
if ( _sitl = = nullptr ) {
_sitl = ( SITL : : SITL * ) AP_Param : : find_object ( " SIM_ " ) ;
}
if ( _sitl & & active_EKF_type ( ) = = EKF_TYPE_SITL ) {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
roll = radians ( fdm . rollDeg ) ;
pitch = radians ( fdm . pitchDeg ) ;
yaw = radians ( fdm . yawDeg ) ;
_dcm_matrix . from_euler ( roll , pitch , yaw ) ;
update_cd_values ( ) ;
update_trig ( ) ;
_gyro_bias . zero ( ) ;
_gyro_estimate = Vector3f ( radians ( fdm . rollRate ) ,
radians ( fdm . pitchRate ) ,
radians ( fdm . yawRate ) ) ;
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES ; i + + ) {
_accel_ef_ekf [ i ] = Vector3f ( fdm . xAccel ,
fdm . yAccel ,
fdm . zAccel ) ;
}
_accel_ef_ekf_blended = _accel_ef_ekf [ 0 ] ;
}
}
# endif // CONFIG_HAL_BOARD
2014-10-31 19:07:18 -03:00
// accelerometer values in the earth frame in m/s/s
const Vector3f & AP_AHRS_NavEKF : : get_accel_ef ( uint8_t i ) const
{
2015-09-23 04:53:44 -03:00
if ( active_EKF_type ( ) = = EKF_TYPE_NONE ) {
2014-10-31 19:07:18 -03:00
return AP_AHRS_DCM : : get_accel_ef ( i ) ;
}
return _accel_ef_ekf [ i ] ;
}
// blended accelerometer values in the earth frame in m/s/s
const Vector3f & AP_AHRS_NavEKF : : get_accel_ef_blended ( void ) const
{
2015-09-23 04:53:44 -03:00
if ( active_EKF_type ( ) = = EKF_TYPE_NONE ) {
2014-10-31 19:07:18 -03:00
return AP_AHRS_DCM : : get_accel_ef_blended ( ) ;
}
return _accel_ef_ekf_blended ;
}
2014-01-01 21:15:58 -04:00
void AP_AHRS_NavEKF : : reset ( bool recover_eulers )
{
AP_AHRS_DCM : : reset ( recover_eulers ) ;
2015-10-20 18:58:30 -03:00
_dcm_attitude ( roll , pitch , yaw ) ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
if ( ekf1_started ) {
2015-09-23 04:29:43 -03:00
ekf1_started = EKF1 . InitialiseFilterBootstrap ( ) ;
2015-09-22 22:40:25 -03:00
}
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
if ( ekf2_started ) {
2015-09-23 04:29:43 -03:00
ekf2_started = EKF2 . InitialiseFilter ( ) ;
2014-01-01 22:05:28 -04:00
}
2014-01-01 21:15:58 -04:00
}
// reset the current attitude, used on new IMU calibration
void AP_AHRS_NavEKF : : reset_attitude ( const float & _roll , const float & _pitch , const float & _yaw )
{
AP_AHRS_DCM : : reset_attitude ( _roll , _pitch , _yaw ) ;
2015-10-20 18:58:30 -03:00
_dcm_attitude ( roll , pitch , yaw ) ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
if ( ekf1_started ) {
ekf1_started = EKF1 . InitialiseFilterBootstrap ( ) ;
}
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
if ( ekf2_started ) {
ekf2_started = EKF2 . InitialiseFilter ( ) ;
2014-01-01 22:05:28 -04:00
}
2014-01-01 21:15:58 -04:00
}
// dead-reckoning support
2014-10-17 22:14:34 -03:00
bool AP_AHRS_NavEKF : : get_position ( struct Location & loc ) const
2014-01-01 21:15:58 -04:00
{
2015-02-20 19:12:53 -04:00
Vector3f ned_pos ;
2016-04-19 04:21:56 -03:00
Location origin ;
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
2016-07-09 08:36:09 -03:00
if ( EKF1 . getLLH ( loc ) & & EKF1 . getPosD ( ned_pos . z ) & & EKF1 . getOriginLLH ( origin ) ) {
2016-04-19 04:21:56 -03:00
// fixup altitude using relative position from EKF origin
loc . alt = origin . alt - ned_pos . z * 100 ;
2015-09-22 22:40:25 -03:00
return true ;
}
break ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2016-07-09 08:36:09 -03:00
if ( EKF2 . getLLH ( loc ) & & EKF2 . getPosD ( - 1 , ned_pos . z ) & & EKF2 . getOriginLLH ( origin ) ) {
2016-04-19 04:21:56 -03:00
// fixup altitude using relative position from EKF origin
loc . alt = origin . alt - ned_pos . z * 100 ;
2015-09-22 22:40:25 -03:00
return true ;
}
break ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
memset ( & loc , 0 , sizeof ( loc ) ) ;
loc . lat = fdm . latitude * 1e7 ;
loc . lng = fdm . longitude * 1e7 ;
loc . alt = fdm . altitude * 100 ;
return true ;
}
# endif
2015-09-22 22:40:25 -03:00
default :
break ;
2014-01-01 22:05:28 -04:00
}
2014-01-01 21:15:58 -04:00
return AP_AHRS_DCM : : get_position ( loc ) ;
}
2014-01-01 22:05:28 -04:00
// status reporting of estimated errors
2015-04-21 10:41:46 -03:00
float AP_AHRS_NavEKF : : get_error_rp ( void ) const
2014-01-01 21:15:58 -04:00
{
return AP_AHRS_DCM : : get_error_rp ( ) ;
}
2015-04-21 10:41:46 -03:00
float AP_AHRS_NavEKF : : get_error_yaw ( void ) const
2014-01-01 21:15:58 -04:00
{
return AP_AHRS_DCM : : get_error_yaw ( ) ;
}
// return a wind estimation vector, in m/s
Vector3f AP_AHRS_NavEKF : : wind_estimate ( void )
{
2014-01-01 22:05:28 -04:00
Vector3f wind ;
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
wind = AP_AHRS_DCM : : wind_estimate ( ) ;
break ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
EKF1 . getWind ( wind ) ;
break ;
2016-04-03 22:08:03 -03:00
# endif
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
wind . zero ( ) ;
break ;
# endif
2016-04-03 22:08:03 -03:00
case EKF_TYPE2 :
default :
EKF2 . getWind ( - 1 , wind ) ;
break ;
2015-09-22 22:40:25 -03:00
}
2014-01-01 22:05:28 -04:00
return wind ;
2014-01-01 21:15:58 -04:00
}
// return an airspeed estimate if available. return true
// if we have an estimate
2014-02-24 21:43:59 -04:00
bool AP_AHRS_NavEKF : : airspeed_estimate ( float * airspeed_ret ) const
2014-01-01 21:15:58 -04:00
{
return AP_AHRS_DCM : : airspeed_estimate ( airspeed_ret ) ;
}
// true if compass is being used
bool AP_AHRS_NavEKF : : use_compass ( void )
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
break ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
return EKF1 . use_compass ( ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
return EKF2 . use_compass ( ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return true ;
# endif
2014-08-24 08:00:56 -03:00
}
2015-09-23 04:29:43 -03:00
return AP_AHRS_DCM : : use_compass ( ) ;
2014-01-01 21:15:58 -04:00
}
2014-01-01 22:47:40 -04:00
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF : : get_secondary_attitude ( Vector3f & eulers )
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
2014-01-01 22:47:40 -04:00
// EKF is secondary
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
EKF1 . getEulerAngles ( eulers ) ;
return ekf1_started ;
2016-04-03 22:08:03 -03:00
# else
EKF2 . getEulerAngles ( - 1 , eulers ) ;
return ekf2_started ;
# endif
2015-09-22 22:40:25 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
default :
// DCM is secondary
eulers = _dcm_attitude ;
2014-01-01 22:47:40 -04:00
return true ;
}
}
// return secondary position solution if available
bool AP_AHRS_NavEKF : : get_secondary_position ( struct Location & loc )
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
// EKF is secondary
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
EKF1 . getLLH ( loc ) ;
return ekf1_started ;
2016-04-03 22:08:03 -03:00
# else
EKF2 . getLLH ( loc ) ;
return ekf2_started ;
# endif
2015-09-22 22:40:25 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
default :
2014-01-01 22:47:40 -04:00
// return DCM position
AP_AHRS_DCM : : get_position ( loc ) ;
return true ;
2015-09-23 04:29:43 -03:00
}
2014-01-01 22:47:40 -04:00
}
2014-01-01 23:25:41 -04:00
// EKF has a better ground speed vector estimate
Vector2f AP_AHRS_NavEKF : : groundspeed_vector ( void )
{
2015-09-22 22:40:25 -03:00
Vector3f vec ;
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
2014-01-01 23:25:41 -04:00
return AP_AHRS_DCM : : groundspeed_vector ( ) ;
2015-09-22 22:40:25 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
EKF1 . getVelNED ( vec ) ;
return Vector2f ( vec . x , vec . y ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-11-07 08:03:41 -04:00
EKF2 . getVelNED ( - 1 , vec ) ;
2015-09-22 22:40:25 -03:00
return Vector2f ( vec . x , vec . y ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
return Vector2f ( fdm . speedN , fdm . speedE ) ;
}
# endif
2014-01-01 23:25:41 -04:00
}
}
2014-03-30 08:00:25 -03:00
void AP_AHRS_NavEKF : : set_home ( const Location & loc )
2014-01-02 07:06:10 -04:00
{
2014-03-30 08:00:25 -03:00
AP_AHRS_DCM : : set_home ( loc ) ;
2014-01-02 07:06:10 -04:00
}
2014-01-03 20:15:34 -04:00
// return true if inertial navigation is active
2015-09-23 04:29:43 -03:00
bool AP_AHRS_NavEKF : : have_inertial_nav ( void ) const
2014-01-03 20:15:34 -04:00
{
2015-09-23 04:53:44 -03:00
return active_EKF_type ( ) ! = EKF_TYPE_NONE ;
2014-01-03 20:15:34 -04:00
}
2014-02-08 04:11:12 -04:00
// return a ground velocity in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF : : get_velocity_NED ( Vector3f & vec ) const
2014-01-03 20:15:34 -04:00
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case EKF_TYPE1 :
EKF1 . getVelNED ( vec ) ;
return true ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-11-07 08:03:41 -04:00
EKF2 . getVelNED ( - 1 , vec ) ;
2014-02-08 04:11:12 -04:00
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
vec = Vector3f ( fdm . speedN , fdm . speedE , fdm . speedD ) ;
return true ;
}
# endif
2014-01-03 20:15:34 -04:00
}
}
2016-01-04 19:57:17 -04:00
// returns the expected NED magnetic field
bool AP_AHRS_NavEKF : : get_mag_field_NED ( Vector3f & vec ) const
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2016-01-04 19:57:17 -04:00
case EKF_TYPE1 :
EKF1 . getMagNED ( vec ) ;
return true ;
2016-04-03 22:08:03 -03:00
# endif
2016-01-04 19:57:17 -04:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2016-01-04 19:57:17 -04:00
EKF2 . getMagNED ( - 1 , vec ) ;
return true ;
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return false ;
# endif
}
}
// returns the estimated magnetic field offsets in body frame
bool AP_AHRS_NavEKF : : get_mag_field_correction ( Vector3f & vec ) const
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2016-01-04 19:57:17 -04:00
case EKF_TYPE1 :
EKF1 . getMagXYZ ( vec ) ;
return true ;
2016-04-03 22:08:03 -03:00
# endif
2016-01-04 19:57:17 -04:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2016-01-04 19:57:17 -04:00
EKF2 . getMagXYZ ( - 1 , vec ) ;
return true ;
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return false ;
# endif
}
}
2015-10-12 06:50:01 -03:00
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
bool AP_AHRS_NavEKF : : get_vert_pos_rate ( float & velocity )
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-12 06:50:01 -03:00
case EKF_TYPE1 :
2015-10-15 02:33:19 -03:00
velocity = EKF1 . getPosDownDerivative ( ) ;
2015-10-12 06:50:01 -03:00
return true ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-12 06:50:01 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-11-07 08:03:41 -04:00
velocity = EKF2 . getPosDownDerivative ( - 1 ) ;
2015-10-12 06:50:01 -03:00
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
velocity = fdm . speedD ;
return true ;
}
# endif
2015-10-12 06:50:01 -03:00
}
}
// get latest height above ground level estimate in metres and a validity flag
bool AP_AHRS_NavEKF : : get_hagl ( float & height ) const
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
2015-10-15 02:10:03 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-12 06:50:01 -03:00
case EKF_TYPE1 :
return EKF1 . getHAGL ( height ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-12 06:50:01 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-10-12 06:50:01 -03:00
return EKF2 . getHAGL ( height ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
height = fdm . altitude - get_home ( ) . alt * 0.01f ;
return true ;
}
# endif
2015-10-12 06:50:01 -03:00
}
}
2014-02-08 04:11:12 -04:00
// return a relative ground position in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF : : get_relative_position_NED ( Vector3f & vec ) const
2014-01-03 20:15:34 -04:00
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2015-09-22 22:40:25 -03:00
case EKF_TYPE_NONE :
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2016-07-09 08:36:09 -03:00
case EKF_TYPE1 : {
Vector2f posNE ;
float posD ;
2016-07-19 21:38:26 -03:00
if ( EKF1 . getPosNE ( posNE ) & & EKF1 . getPosD ( posD ) ) {
// position is valid
vec . x = posNE . x ;
vec . y = posNE . y ;
vec . z = posD ;
return true ;
}
return false ;
2016-07-09 08:36:09 -03:00
}
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2016-07-09 08:36:09 -03:00
default : {
Vector2f posNE ;
float posD ;
2016-07-19 21:38:26 -03:00
if ( EKF2 . getPosNE ( - 1 , posNE ) & & EKF2 . getPosD ( - 1 , posD ) ) {
// position is valid
vec . x = posNE . x ;
vec . y = posNE . y ;
vec . z = posD ;
return true ;
}
return false ;
2016-07-09 08:36:09 -03:00
}
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
Location loc ;
get_position ( loc ) ;
Vector2f diff2d = location_diff ( get_home ( ) , loc ) ;
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
vec = Vector3f ( diff2d . x , diff2d . y ,
- ( fdm . altitude - get_home ( ) . alt * 0.01f ) ) ;
return true ;
}
# endif
2014-01-03 20:15:34 -04:00
}
}
2016-07-11 20:36:41 -03:00
// write a relative ground position estimate in meters, North/East order
// return true if estimate is valid
2016-07-09 08:36:09 -03:00
bool AP_AHRS_NavEKF : : get_relative_position_NE ( Vector2f & posNE ) const
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
# if AP_AHRS_WITH_EKF1
case EKF_TYPE1 : {
bool position_is_valid = EKF1 . getPosNE ( posNE ) ;
return position_is_valid ;
}
# endif
case EKF_TYPE2 :
default : {
bool position_is_valid = EKF2 . getPosNE ( - 1 , posNE ) ;
return position_is_valid ;
}
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
Location loc ;
get_position ( loc ) ;
posNE = location_diff ( get_home ( ) , loc ) ;
return true ;
}
# endif
}
}
2016-07-11 20:36:41 -03:00
// write a relative ground position in meters, Down
2016-07-09 08:36:09 -03:00
// return true if the estimate is valid
bool AP_AHRS_NavEKF : : get_relative_position_D ( float & posD ) const
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
# if AP_AHRS_WITH_EKF1
case EKF_TYPE1 : {
bool position_is_valid = EKF1 . getPosD ( posD ) ;
return position_is_valid ;
}
# endif
case EKF_TYPE2 :
default : {
bool position_is_valid = EKF2 . getPosD ( - 1 , posD ) ;
return position_is_valid ;
}
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
posD = - ( fdm . altitude - get_home ( ) . alt * 0.01f ) ;
return true ;
}
# endif
}
}
2015-09-22 22:40:25 -03:00
/*
canonicalise _ekf_type , forcing it to be 0 , 1 or 2
*/
uint8_t AP_AHRS_NavEKF : : ekf_type ( void ) const
2014-02-14 18:24:12 -04:00
{
2015-09-22 22:40:25 -03:00
uint8_t type = _ekf_type ;
2015-10-13 15:08:49 -03:00
if ( always_use_EKF ( ) & & type = = 0 ) {
2015-09-22 22:40:25 -03:00
type = 1 ;
}
2016-04-03 22:08:03 -03:00
# if !AP_AHRS_WITH_EKF1
if ( type = = 1 ) {
type = 2 ;
}
# endif
2015-09-22 22:40:25 -03:00
// check for invalid type
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if ( type > 2 & & type ! = EKF_TYPE_SITL ) {
2016-04-03 22:08:03 -03:00
type = 2 ;
2015-11-20 04:34:30 -04:00
}
# else
2015-09-22 22:40:25 -03:00
if ( type > 2 ) {
2016-04-03 22:08:03 -03:00
type = 2 ;
2015-09-22 22:40:25 -03:00
}
2015-11-20 04:34:30 -04:00
# endif
2015-09-22 22:40:25 -03:00
return type ;
}
2015-09-23 04:53:44 -03:00
AP_AHRS_NavEKF : : EKF_TYPE AP_AHRS_NavEKF : : active_EKF_type ( void ) const
2015-09-22 22:40:25 -03:00
{
EKF_TYPE ret = EKF_TYPE_NONE ;
switch ( ekf_type ( ) ) {
case 0 :
return EKF_TYPE_NONE ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case 1 : {
// do we have an EKF yet?
if ( ! ekf1_started ) {
return EKF_TYPE_NONE ;
}
2015-10-13 15:08:49 -03:00
if ( always_use_EKF ( ) ) {
2016-05-10 03:05:50 -03:00
uint16_t ekf_faults ;
2015-10-13 15:08:49 -03:00
EKF1 . getFilterFaults ( ekf_faults ) ;
if ( ekf_faults = = 0 ) {
ret = EKF_TYPE1 ;
}
} else if ( EKF1 . healthy ( ) ) {
2015-09-23 04:29:43 -03:00
ret = EKF_TYPE1 ;
2015-09-22 22:40:25 -03:00
}
break ;
2015-04-07 20:11:24 -03:00
}
2016-04-03 22:08:03 -03:00
# endif
2015-05-19 02:15:44 -03:00
2015-09-22 22:40:25 -03:00
case 2 : {
// do we have an EKF2 yet?
if ( ! ekf2_started ) {
return EKF_TYPE_NONE ;
}
2015-10-13 15:08:49 -03:00
if ( always_use_EKF ( ) ) {
2016-05-10 03:05:50 -03:00
uint16_t ekf2_faults ;
2015-11-07 08:03:41 -04:00
EKF2 . getFilterFaults ( - 1 , ekf2_faults ) ;
2015-10-13 15:08:49 -03:00
if ( ekf2_faults = = 0 ) {
ret = EKF_TYPE2 ;
}
} else if ( EKF2 . healthy ( ) ) {
2015-09-22 22:40:25 -03:00
ret = EKF_TYPE2 ;
}
break ;
}
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
ret = EKF_TYPE_SITL ;
break ;
# endif
2015-09-22 22:40:25 -03:00
}
2016-03-04 17:52:40 -04:00
/*
fixed wing and rover when in fly_forward mode will fall back to
DCM if the EKF doesn ' t have GPS . This is the safest option as
DCM is very robust
*/
2015-09-22 22:40:25 -03:00
if ( ret ! = EKF_TYPE_NONE & &
2016-03-04 17:52:40 -04:00
( _vehicle_class = = AHRS_VEHICLE_FIXED_WING | |
_vehicle_class = = AHRS_VEHICLE_GROUND ) & &
_flags . fly_forward ) {
2015-05-19 02:15:44 -03:00
nav_filter_status filt_state ;
2016-04-03 22:08:03 -03:00
if ( ret = = EKF_TYPE2 ) {
EKF2 . getFilterStatus ( - 1 , filt_state ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
} else if ( ret = = EKF_TYPE_SITL ) {
get_filter_status ( filt_state ) ;
# endif
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
} else {
2016-04-03 22:08:03 -03:00
EKF1 . getFilterStatus ( filt_state ) ;
# endif
2015-09-22 22:40:25 -03:00
}
2015-05-19 02:15:44 -03:00
if ( hal . util - > get_soft_armed ( ) & & ! filt_state . flags . using_gps & & _gps . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
// if the EKF is not fusing GPS and we have a 3D lock, then
// plane and rover would prefer to use the GPS position from
// DCM. This is a safety net while some issues with the EKF
// get sorted out
2015-09-22 22:40:25 -03:00
return EKF_TYPE_NONE ;
2015-05-19 02:15:44 -03:00
}
if ( hal . util - > get_soft_armed ( ) & & filt_state . flags . const_pos_mode ) {
2015-09-22 22:40:25 -03:00
return EKF_TYPE_NONE ;
2015-05-19 02:15:44 -03:00
}
if ( ! filt_state . flags . attitude | |
2016-05-26 21:18:53 -03:00
! filt_state . flags . vert_vel | |
! filt_state . flags . vert_pos ) {
return EKF_TYPE_NONE ;
}
if ( ! filt_state . flags . horiz_vel | |
! filt_state . flags . horiz_pos_abs ) {
if ( ( ! _compass | | ! _compass - > use_for_yaw ( ) ) & &
_gps . status ( ) > = AP_GPS : : GPS_OK_FIX_3D & &
_gps . ground_speed ( ) < 2 ) {
/*
special handling for non - compass mode when sitting
still . The EKF may not yet have aligned its yaw . We
accept EKF as healthy to allow arming . Once we reach
speed the EKF should get yaw alignment
*/
if ( filt_state . flags . pred_horiz_pos_abs & &
filt_state . flags . pred_horiz_pos_rel ) {
return ret ;
}
}
2015-09-22 22:40:25 -03:00
return EKF_TYPE_NONE ;
2015-05-19 02:15:44 -03:00
}
2015-04-07 20:11:24 -03:00
}
return ret ;
2014-02-14 18:24:12 -04:00
}
2014-05-15 04:09:18 -03:00
/*
check if the AHRS subsystem is healthy
*/
2015-01-31 22:31:24 -04:00
bool AP_AHRS_NavEKF : : healthy ( void ) const
2014-05-15 04:09:18 -03:00
{
2015-05-13 02:37:17 -03:00
// If EKF is started we switch away if it reports unhealthy. This could be due to bad
// sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
// an internal processing error, but not for bad sensor data.
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
return AP_AHRS_DCM : : healthy ( ) ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-22 22:40:25 -03:00
case 1 : {
bool ret = ekf1_started & & EKF1 . healthy ( ) ;
2015-05-19 02:11:31 -03:00
if ( ! ret ) {
return false ;
}
if ( ( _vehicle_class = = AHRS_VEHICLE_FIXED_WING | |
2015-09-23 04:29:43 -03:00
_vehicle_class = = AHRS_VEHICLE_GROUND ) & &
2015-09-23 04:53:44 -03:00
active_EKF_type ( ) ! = EKF_TYPE1 ) {
2015-05-19 02:11:31 -03:00
// on fixed wing we want to be using EKF to be considered
// healthy if EKF is enabled
return false ;
}
return true ;
2014-05-15 04:09:18 -03:00
}
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
case 2 : {
bool ret = ekf2_started & & EKF2 . healthy ( ) ;
if ( ! ret ) {
return false ;
}
if ( ( _vehicle_class = = AHRS_VEHICLE_FIXED_WING | |
2015-09-23 04:29:43 -03:00
_vehicle_class = = AHRS_VEHICLE_GROUND ) & &
2015-09-23 04:53:44 -03:00
active_EKF_type ( ) ! = EKF_TYPE2 ) {
2015-09-22 22:40:25 -03:00
// on fixed wing we want to be using EKF to be considered
// healthy if EKF is enabled
return false ;
}
return true ;
}
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return true ;
# endif
2015-09-22 22:40:25 -03:00
}
2015-09-23 04:29:43 -03:00
return AP_AHRS_DCM : : healthy ( ) ;
2014-05-15 04:09:18 -03:00
}
2015-03-19 02:58:13 -03:00
void AP_AHRS_NavEKF : : set_ekf_use ( bool setting )
{
2015-09-22 22:40:25 -03:00
_ekf_type . set ( setting ? 1 : 0 ) ;
2015-03-19 02:58:13 -03:00
}
2014-10-02 01:43:46 -03:00
// true if the AHRS has completed initialisation
bool AP_AHRS_NavEKF : : initialised ( void ) const
2014-09-29 05:37:14 -03:00
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
return true ;
case 1 :
// initialisation complete 10sec after ekf has started
2015-11-19 23:06:30 -04:00
return ( ekf1_started & & ( AP_HAL : : millis ( ) - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS ) ) ;
2015-09-22 22:40:25 -03:00
case 2 :
2016-04-03 22:08:03 -03:00
default :
2015-09-22 22:40:25 -03:00
// initialisation complete 10sec after ekf has started
2015-11-19 23:06:30 -04:00
return ( ekf2_started & & ( AP_HAL : : millis ( ) - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS ) ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return true ;
# endif
2015-09-23 04:29:43 -03:00
}
2014-10-02 01:43:46 -03:00
} ;
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 AP_AHRS_NavEKF : : get_filter_status ( nav_filter_status & status ) const
{
switch ( ekf_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
2015-10-15 02:10:03 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-12 06:50:01 -03:00
case EKF_TYPE1 :
EKF1 . getFilterStatus ( status ) ;
2015-10-15 02:10:03 -03:00
return true ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-15 02:10:03 -03:00
2015-10-12 06:50:01 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-11-07 08:03:41 -04:00
EKF2 . getFilterStatus ( - 1 , status ) ;
2015-10-15 02:10:03 -03:00
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
memset ( & status , 0 , sizeof ( status ) ) ;
status . flags . attitude = 1 ;
status . flags . horiz_vel = 1 ;
status . flags . vert_vel = 1 ;
status . flags . horiz_pos_rel = 1 ;
status . flags . horiz_pos_abs = 1 ;
status . flags . vert_pos = 1 ;
status . flags . pred_horiz_pos_rel = 1 ;
status . flags . pred_horiz_pos_abs = 1 ;
status . flags . using_gps = 1 ;
return true ;
# endif
2015-10-12 06:50:01 -03:00
}
2015-10-15 02:10:03 -03:00
2015-10-12 06:50:01 -03:00
}
2014-08-06 21:31:24 -03:00
// write optical flow data to EKF
2015-04-15 18:43:42 -03:00
void AP_AHRS_NavEKF : : writeOptFlowMeas ( uint8_t & rawFlowQuality , Vector2f & rawFlowRates , Vector2f & rawGyroRates , uint32_t & msecFlowMeas )
2014-08-06 21:31:24 -03:00
{
2015-09-22 22:40:25 -03:00
EKF1 . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas ) ;
EKF2 . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas ) ;
2014-08-06 21:31:24 -03:00
}
2014-11-12 14:49:15 -04:00
2016-05-12 14:00:11 -03:00
// inhibit GPS usage
2014-11-12 14:49:15 -04:00
uint8_t AP_AHRS_NavEKF : : setInhibitGPS ( void )
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
case 1 :
return EKF1 . setInhibitGPS ( ) ;
case 2 :
2016-04-03 22:08:03 -03:00
default :
2015-09-22 22:40:25 -03:00
return EKF2 . setInhibitGPS ( ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return false ;
# endif
2015-09-22 22:40:25 -03:00
}
2014-11-12 14:49:15 -04:00
}
2014-11-15 19:36:33 -04:00
// get speed limit
2014-11-15 21:42:30 -04:00
void AP_AHRS_NavEKF : : getEkfControlLimits ( float & ekfGndSpdLimit , float & ekfNavVelGainScaler )
2014-11-15 19:36:33 -04:00
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
case 1 :
EKF1 . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
break ;
case 2 :
2016-04-03 22:08:03 -03:00
default :
2015-09-22 22:40:25 -03:00
EKF2 . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
break ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
// same as EKF1 for no optical flow
ekfGndSpdLimit = 400.0f ;
ekfNavVelGainScaler = 1.0f ;
break ;
# endif
2015-09-22 22:40:25 -03:00
}
2014-11-15 19:36:33 -04:00
}
2015-03-16 18:29:11 -03:00
// get compass offset estimates
// true if offsets are valid
2016-03-29 17:06:56 -03:00
bool AP_AHRS_NavEKF : : getMagOffsets ( uint8_t mag_idx , Vector3f & magOffsets )
2015-03-16 18:29:11 -03:00
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
case 1 :
2016-03-29 17:06:56 -03:00
return EKF1 . getMagOffsets ( mag_idx , magOffsets ) ;
2015-09-22 22:40:25 -03:00
case 2 :
2016-04-03 22:08:03 -03:00
default :
2016-03-29 17:06:56 -03:00
return EKF2 . getMagOffsets ( mag_idx , magOffsets ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
magOffsets . zero ( ) ;
return true ;
# endif
2015-09-22 22:40:25 -03:00
}
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 * AP_AHRS_NavEKF : : prearm_failure_reason ( void ) const
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
return nullptr ;
case 1 :
return EKF1 . prearm_failure_reason ( ) ;
case 2 :
2015-10-30 01:00:22 -03:00
return EKF2 . prearm_failure_reason ( ) ;
2015-09-08 02:50:22 -03:00
}
return nullptr ;
}
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
2015-10-29 05:31:02 -03:00
uint32_t AP_AHRS_NavEKF : : getLastYawResetAngle ( float & yawAng ) const
2015-09-23 04:46:51 -03:00
{
switch ( ekf_type ( ) ) {
case 1 :
return EKF1 . getLastYawResetAngle ( yawAng ) ;
case 2 :
return EKF2 . getLastYawResetAngle ( yawAng ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return 0 ;
# endif
2015-09-23 04:46:51 -03:00
}
2015-10-29 05:31:02 -03:00
return 0 ;
2015-09-23 04:46:51 -03:00
}
2015-10-29 02:18:04 -03:00
// return the amount of NE position change in metres due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
2015-10-29 05:34:41 -03:00
uint32_t AP_AHRS_NavEKF : : getLastPosNorthEastReset ( Vector2f & pos ) const
2015-10-29 02:18:04 -03:00
{
switch ( ekf_type ( ) ) {
case 1 :
return EKF1 . getLastPosNorthEastReset ( pos ) ;
case 2 :
return EKF2 . getLastPosNorthEastReset ( pos ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return 0 ;
# endif
2015-10-29 02:18:04 -03:00
}
return 0 ;
}
// return the amount of NE velocity change in metres/sec due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
2015-10-29 05:34:41 -03:00
uint32_t AP_AHRS_NavEKF : : getLastVelNorthEastReset ( Vector2f & vel ) const
2015-10-29 02:18:04 -03:00
{
switch ( ekf_type ( ) ) {
case 1 :
return EKF1 . getLastVelNorthEastReset ( vel ) ;
case 2 :
return EKF2 . getLastVelNorthEastReset ( vel ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return 0 ;
# endif
2015-10-29 02:18:04 -03:00
}
return 0 ;
}
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
bool AP_AHRS_NavEKF : : resetHeightDatum ( void )
{
switch ( ekf_type ( ) ) {
case 1 :
EKF2 . resetHeightDatum ( ) ;
return EKF1 . resetHeightDatum ( ) ;
case 2 :
EKF1 . resetHeightDatum ( ) ;
return EKF2 . resetHeightDatum ( ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return false ;
# endif
2015-09-23 04:46:51 -03:00
}
2015-10-15 02:10:03 -03:00
return false ;
2015-09-23 04:46:51 -03:00
}
2015-09-28 21:58:54 -03:00
// send a EKF_STATUS_REPORT for current EKF
void AP_AHRS_NavEKF : : send_ekf_status_report ( mavlink_channel_t chan )
{
switch ( active_EKF_type ( ) ) {
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-09-28 21:58:54 -03:00
case EKF_TYPE1 :
return EKF1 . send_status_report ( chan ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-28 21:58:54 -03:00
2016-06-19 09:02:25 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
// send zero status report
mavlink_msg_ekf_status_report_send ( chan , 0 , 0 , 0 , 0 , 0 , 0 ) ;
break ;
# endif
2015-09-28 21:58:54 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-09-28 21:58:54 -03:00
return EKF2 . send_status_report ( chan ) ;
}
}
2015-10-15 02:10:03 -03:00
// passes a reference to the location of the inertial navigation origin
// in WGS-84 coordinates
// returns a boolean true when the inertial navigation origin has been set
bool AP_AHRS_NavEKF : : get_origin ( Location & ret ) const
2015-10-12 06:50:01 -03:00
{
switch ( ekf_type ( ) ) {
case EKF_TYPE_NONE :
2015-10-15 02:10:03 -03:00
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-12 06:50:01 -03:00
case EKF_TYPE1 :
if ( ! EKF1 . getOriginLLH ( ret ) ) {
2015-10-15 02:10:03 -03:00
return false ;
2015-10-12 06:50:01 -03:00
}
2015-10-15 02:10:03 -03:00
return true ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-15 02:10:03 -03:00
2015-10-12 06:50:01 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-10-12 06:50:01 -03:00
if ( ! EKF2 . getOriginLLH ( ret ) ) {
2015-10-15 02:10:03 -03:00
return false ;
2015-10-12 06:50:01 -03:00
}
2015-10-15 02:10:03 -03:00
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
ret = get_home ( ) ;
return ret . lat ! = 0 | | ret . lng ! = 0 ;
# endif
2015-10-12 06:50:01 -03:00
}
}
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag
// this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required
bool AP_AHRS_NavEKF : : get_hgt_ctrl_limit ( float & limit ) const
{
switch ( ekf_type ( ) ) {
case EKF_TYPE_NONE :
// We are not using an EKF so no limiting applies
return false ;
2015-10-15 02:10:03 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-12 06:50:01 -03:00
case EKF_TYPE1 :
return EKF1 . getHeightControlLimit ( limit ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-15 02:10:03 -03:00
2015-10-12 06:50:01 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-10-12 06:50:01 -03:00
return EKF2 . getHeightControlLimit ( limit ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return false ;
# endif
2015-10-12 06:50:01 -03:00
}
}
2015-10-15 02:10:03 -03:00
// get_location - updates the provided location with the latest calculated location
2015-10-12 06:50:01 -03:00
// returns true on success (i.e. the EKF knows it's latest position), false on failure
bool AP_AHRS_NavEKF : : get_location ( struct Location & loc ) const
{
switch ( ekf_type ( ) ) {
case EKF_TYPE_NONE :
2015-10-15 02:10:03 -03:00
// We are not using an EKF so no data
2015-10-12 06:50:01 -03:00
return false ;
2015-10-15 02:10:03 -03:00
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-12 06:50:01 -03:00
case EKF_TYPE1 :
return EKF1 . getLLH ( loc ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-15 02:10:03 -03:00
2015-10-12 06:50:01 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-10-12 06:50:01 -03:00
return EKF2 . getLLH ( loc ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return get_position ( loc ) ;
# endif
2015-10-12 06:50:01 -03:00
}
}
2015-10-17 02:49:50 -03:00
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accpeted by the filter
// boolean false is returned if variances are not available
bool AP_AHRS_NavEKF : : get_variances ( float & velVar , float & posVar , float & hgtVar , Vector3f & magVar , float & tasVar , Vector2f & offset ) const
{
switch ( ekf_type ( ) ) {
case EKF_TYPE_NONE :
// We are not using an EKF so no data
return false ;
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-10-17 02:49:50 -03:00
case EKF_TYPE1 :
// use EKF to get variance
EKF1 . getVariances ( velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
return true ;
2016-04-03 22:08:03 -03:00
# endif
2015-10-17 02:49:50 -03:00
case EKF_TYPE2 :
2016-04-03 22:08:03 -03:00
default :
2015-10-17 02:49:50 -03:00
// use EKF to get variance
2015-11-07 08:03:41 -04:00
EKF2 . getVariances ( - 1 , velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
2015-10-17 02:49:50 -03:00
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
velVar = 0 ;
posVar = 0 ;
hgtVar = 0 ;
magVar . zero ( ) ;
tasVar = 0 ;
offset . zero ( ) ;
return true ;
# endif
2015-10-17 02:49:50 -03:00
}
}
2015-12-22 15:18:03 -04:00
void AP_AHRS_NavEKF : : setTakeoffExpected ( bool val )
{
switch ( ekf_type ( ) ) {
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-12-22 15:18:03 -04:00
case EKF_TYPE1 :
EKF1 . setTakeoffExpected ( val ) ;
break ;
2016-04-03 22:08:03 -03:00
# endif
2015-12-22 15:18:03 -04:00
case EKF_TYPE2 :
EKF2 . setTakeoffExpected ( val ) ;
break ;
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
break ;
# endif
}
}
void AP_AHRS_NavEKF : : setTouchdownExpected ( bool val )
{
switch ( ekf_type ( ) ) {
2016-04-03 22:08:03 -03:00
# if AP_AHRS_WITH_EKF1
2015-12-22 15:18:03 -04:00
case EKF_TYPE1 :
EKF1 . setTouchdownExpected ( val ) ;
break ;
2016-04-03 22:08:03 -03:00
# endif
2015-12-22 15:18:03 -04:00
case EKF_TYPE2 :
EKF2 . setTouchdownExpected ( val ) ;
break ;
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
break ;
# endif
}
}
2016-01-26 17:28:21 -04:00
bool AP_AHRS_NavEKF : : getGpsGlitchStatus ( )
{
nav_filter_status ekf_status ;
get_filter_status ( ekf_status ) ;
return ekf_status . flags . gps_glitching ;
}
2015-10-17 02:49:50 -03:00
2016-05-04 01:02:12 -03:00
// is the EKF backend doing its own sensor logging?
bool AP_AHRS_NavEKF : : have_ekf_logging ( void ) const
{
2016-05-04 20:24:19 -03:00
switch ( ekf_type ( ) ) {
case 2 :
return EKF2 . have_ekf_logging ( ) ;
default :
break ;
}
2016-05-04 01:02:12 -03:00
return false ;
}
2014-01-01 21:15:58 -04:00
# endif // AP_AHRS_NAVEKF_AVAILABLE
2014-02-14 18:24:12 -04:00