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"
2017-02-11 04:15:34 -04:00
# include "AP_AHRS_View.h"
2015-08-11 03:28:42 -03:00
# 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
2018-03-10 05:35:03 -04:00
AP_AHRS_NavEKF : : AP_AHRS_NavEKF ( NavEKF2 & _EKF2 ,
2017-12-01 19:50:46 -04:00
NavEKF3 & _EKF3 ,
Flags flags ) :
2018-03-10 05:35:03 -04:00
AP_AHRS_DCM ( ) ,
2015-11-20 04:34:30 -04:00
EKF2 ( _EKF2 ) ,
2016-07-14 02:08:42 -03:00
EKF3 ( _EKF3 ) ,
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 ( ) ;
}
2017-02-08 02:35:33 -04:00
return _gyro_drift ;
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 )
{
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2014-10-28 08:22:48 -03:00
// 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
EKF2 . resetGyroBias ( ) ;
2016-07-14 02:08:42 -03:00
EKF3 . resetGyroBias ( ) ;
2014-10-28 08:22:48 -03:00
}
2017-04-02 22:03:45 -03:00
void AP_AHRS_NavEKF : : update ( bool skip_ins_update )
2015-09-22 22:40:25 -03:00
{
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2018-05-04 05:29:43 -03:00
// drop back to normal priority if we were boosted by the INS
// calling delay_microseconds_boost()
hal . scheduler - > boost_end ( ) ;
2016-06-28 04:36:12 -03:00
// EKF1 is no longer supported - handle case where it is selected
2016-04-03 22:08:03 -03:00
if ( _ekf_type = = 1 ) {
_ekf_type . set ( 2 ) ;
}
2017-12-12 21:27:55 -04:00
2018-06-06 21:01:02 -03:00
update_DCM ( skip_ins_update ) ;
2017-12-12 21:27:55 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
update_SITL ( ) ;
# endif
2017-04-28 21:26:29 -03:00
if ( _ekf_type = = 2 ) {
// if EK2 is primary then run EKF2 first to give it CPU
// priority
update_EKF2 ( ) ;
update_EKF3 ( ) ;
} else {
// otherwise run EKF3 first
update_EKF3 ( ) ;
update_EKF2 ( ) ;
}
2016-07-07 05:26:53 -03:00
2018-02-09 19:09:09 -04:00
# if AP_MODULE_SUPPORTED
2016-07-07 05:26:53 -03:00
// call AHRS_update hook if any
AP_Module : : call_hook_AHRS_update ( * this ) ;
2018-02-09 19:09:09 -04:00
# endif
2017-01-17 12:50:38 -04:00
// push gyros if optical flow present
if ( hal . opticalflow ) {
2017-01-23 06:36:34 -04:00
const Vector3f & exported_gyro_bias = get_gyro_drift ( ) ;
2017-01-17 12:50:38 -04:00
hal . opticalflow - > push_gyro_bias ( exported_gyro_bias . x , exported_gyro_bias . y ) ;
}
2017-02-11 04:15:34 -04:00
if ( _view ! = nullptr ) {
// update optional alternative attitude view
2017-04-02 22:03:45 -03:00
_view - > update ( skip_ins_update ) ;
2017-02-11 04:15:34 -04:00
}
2015-09-22 22:40:25 -03:00
}
2017-04-02 22:03:45 -03:00
void AP_AHRS_NavEKF : : update_DCM ( bool skip_ins_update )
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
2017-04-02 22:03:45 -03:00
AP_AHRS_DCM : : update ( skip_ins_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_EKF2 ( void )
{
2016-12-19 00:38:44 -04:00
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-12-19 00:38:44 -04:00
if ( AP_HAL : : millis ( ) - start_time_ms > startup_delay_ms | | _force_ekf ) {
_ekf2_started = EKF2 . InitialiseFilter ( ) ;
if ( _force_ekf ) {
2016-05-03 03:34:05 -03:00
return ;
}
2014-01-01 22:05:28 -04:00
}
}
2016-12-19 00:38:44 -04:00
if ( _ekf2_started ) {
2015-09-22 22:40:25 -03:00
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
2017-02-08 02:35:33 -04:00
// Use the primary EKF to select the primary gyro
2017-12-03 06:10:19 -04:00
const int8_t primary_imu = EKF2 . getPrimaryCoreIMUIndex ( ) ;
2014-07-13 08:56:39 -03:00
2018-03-10 05:35:03 -04:00
const AP_InertialSensor & _ins = AP : : ins ( ) ;
2017-02-08 02:35:33 -04:00
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
_gyro_drift . zero ( ) ;
EKF2 . getGyroBias ( - 1 , _gyro_drift ) ;
_gyro_drift = - _gyro_drift ;
2016-09-03 04:12:39 -03:00
2017-02-08 02:35:33 -04:00
// calculate corrected gyro estimate for get_gyro()
_gyro_estimate . zero ( ) ;
2016-09-03 04:12:39 -03:00
if ( primary_imu = = - 1 ) {
2017-02-08 02:35:33 -04:00
// the primary IMU is undefined so use an uncorrected default value from the INS library
2016-09-03 04:12:39 -03:00
_gyro_estimate = _ins . get_gyro ( ) ;
} else {
2017-02-08 02:35:33 -04:00
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins . get_gyro ( primary_imu ) + _gyro_drift ;
2014-07-13 08:56:39 -03:00
}
2014-10-31 19:07:18 -03:00
2016-06-28 04:36:12 -03:00
// get z accel bias estimate from active EKF (this is usually for the primary IMU)
2016-09-03 04:12:39 -03:00
float abias = 0 ;
2015-11-07 08:03:41 -04:00
EKF2 . getAccelZBias ( - 1 , abias ) ;
2015-03-28 04:08:11 -03:00
2016-09-03 04:12:39 -03:00
// This EKF is currently using primary_imu, and abias applies to only that IMU
2014-10-31 19:07:18 -03:00
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 ) ;
2016-09-03 04:12:39 -03:00
if ( i = = primary_imu ) {
2015-09-23 22:10:11 -03:00
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 ) ) {
2016-07-26 15:06:29 -03:00
_accel_ef_ekf [ i ] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * accel ;
2014-10-31 19:07:18 -03:00
}
}
2016-09-03 04:12:39 -03:00
_accel_ef_ekf_blended = _accel_ef_ekf [ primary_imu > = 0 ? primary_imu : _ins . get_primary_accel ( ) ] ;
2017-06-01 19:02:59 -03:00
nav_filter_status filt_state ;
EKF2 . getFilterStatus ( - 1 , filt_state ) ;
AP_Notify : : flags . gps_fusion = filt_state . flags . using_gps ; // Drives AP_Notify flag for usable GPS.
2017-07-26 05:31:42 -03:00
AP_Notify : : flags . gps_glitching = filt_state . flags . gps_glitching ;
2017-06-26 22:27:21 -03:00
AP_Notify : : flags . have_pos_abs = filt_state . flags . horiz_pos_abs ;
2014-01-01 22:05:28 -04:00
}
}
2014-01-01 21:15:58 -04:00
}
2016-07-14 02:08:42 -03:00
void AP_AHRS_NavEKF : : update_EKF3 ( void )
{
2016-12-19 00:38:44 -04:00
if ( ! _ekf3_started ) {
2016-07-14 02:08:42 -03:00
// wait 1 second for DCM to output a valid tilt error estimate
if ( start_time_ms = = 0 ) {
start_time_ms = AP_HAL : : millis ( ) ;
}
2016-12-19 00:38:44 -04:00
if ( AP_HAL : : millis ( ) - start_time_ms > startup_delay_ms | | _force_ekf ) {
_ekf3_started = EKF3 . InitialiseFilter ( ) ;
if ( _force_ekf ) {
2016-07-14 02:08:42 -03:00
return ;
}
}
}
2016-12-19 00:38:44 -04:00
if ( _ekf3_started ) {
2016-07-14 02:08:42 -03:00
EKF3 . UpdateFilter ( ) ;
2016-06-28 04:36:12 -03:00
if ( active_EKF_type ( ) = = EKF_TYPE3 ) {
2016-07-14 02:08:42 -03:00
Vector3f eulers ;
EKF3 . getRotationBodyToNED ( _dcm_matrix ) ;
EKF3 . getEulerAngles ( - 1 , eulers ) ;
roll = eulers . x ;
pitch = eulers . y ;
yaw = eulers . z ;
update_cd_values ( ) ;
update_trig ( ) ;
2018-03-10 05:35:03 -04:00
const AP_InertialSensor & _ins = AP : : ins ( ) ;
2017-02-08 02:35:33 -04:00
// Use the primary EKF to select the primary gyro
2017-12-03 06:10:19 -04:00
const int8_t primary_imu = EKF3 . getPrimaryCoreIMUIndex ( ) ;
2017-02-08 02:35:33 -04:00
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
_gyro_drift . zero ( ) ;
EKF3 . getGyroBias ( - 1 , _gyro_drift ) ;
_gyro_drift = - _gyro_drift ;
2016-07-14 02:08:42 -03:00
2017-02-08 02:35:33 -04:00
// calculate corrected gyro estimate for get_gyro()
2016-07-14 02:08:42 -03:00
_gyro_estimate . zero ( ) ;
2017-02-08 02:35:33 -04:00
if ( primary_imu = = - 1 ) {
// the primary IMU is undefined so use an uncorrected default value from the INS library
_gyro_estimate = _ins . get_gyro ( ) ;
} else {
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins . get_gyro ( primary_imu ) + _gyro_drift ;
2016-07-14 02:08:42 -03:00
}
2016-06-28 04:36:12 -03:00
// get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU)
Vector3f abias ;
EKF3 . getAccelBias ( - 1 , abias ) ;
2016-07-14 02:08:42 -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
// update _accel_ef_ekf
for ( uint8_t i = 0 ; i < _ins . get_accel_count ( ) ; i + + ) {
Vector3f accel = _ins . get_accel ( i ) ;
if ( i = = _ins . get_primary_accel ( ) ) {
2016-06-28 04:36:12 -03:00
accel - = abias ;
2016-07-14 02:08:42 -03:00
}
if ( _ins . get_accel_health ( i ) ) {
_accel_ef_ekf [ i ] = _dcm_matrix * accel ;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf [ _ins . get_primary_accel ( ) ] ;
2017-06-01 19:02:59 -03:00
nav_filter_status filt_state ;
EKF3 . getFilterStatus ( - 1 , filt_state ) ;
AP_Notify : : flags . gps_fusion = filt_state . flags . using_gps ; // Drives AP_Notify flag for usable GPS.
2017-07-26 05:31:42 -03:00
AP_Notify : : flags . gps_glitching = filt_state . flags . gps_glitching ;
2017-06-26 22:27:21 -03:00
AP_Notify : : flags . have_pos_abs = filt_state . flags . horiz_pos_abs ;
2016-07-14 02:08:42 -03: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 ) {
2018-05-10 20:32:06 -03:00
_sitl = AP : : sitl ( ) ;
2017-03-21 19:38:50 -03:00
if ( _sitl = = nullptr ) {
return ;
}
2015-11-20 04:34:30 -04:00
}
2017-03-21 19:38:50 -03:00
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
2018-07-19 05:09:17 -03:00
const AP_InertialSensor & _ins = AP : : ins ( ) ;
2017-03-21 19:38:50 -03:00
if ( active_EKF_type ( ) = = EKF_TYPE_SITL ) {
2015-11-20 04:34:30 -04:00
roll = radians ( fdm . rollDeg ) ;
pitch = radians ( fdm . pitchDeg ) ;
yaw = radians ( fdm . yawDeg ) ;
2017-04-15 08:21:09 -03:00
fdm . quaternion . rotation_matrix ( _dcm_matrix ) ;
2015-11-20 04:34:30 -04:00
update_cd_values ( ) ;
update_trig ( ) ;
2017-02-08 02:35:33 -04:00
_gyro_drift . zero ( ) ;
2015-11-20 04:34:30 -04:00
2018-07-19 05:09:17 -03:00
_gyro_estimate = _ins . get_gyro ( ) ;
2015-11-20 04:34:30 -04:00
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES ; i + + ) {
2018-07-19 05:09:17 -03:00
const Vector3f & accel = _ins . get_accel ( i ) ;
2017-05-05 06:49:20 -03:00
_accel_ef_ekf [ i ] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * accel ;
2015-11-20 04:34:30 -04:00
}
_accel_ef_ekf_blended = _accel_ef_ekf [ 0 ] ;
2017-03-21 19:38:50 -03:00
}
2017-04-30 21:09:57 -03:00
if ( _sitl - > odom_enable ) {
// use SITL states to write body frame odometry data at 20Hz
uint32_t timeStamp_ms = AP_HAL : : millis ( ) ;
if ( timeStamp_ms - _last_body_odm_update_ms > 50 ) {
const float quality = 100.0f ;
2017-12-02 09:02:37 -04:00
const Vector3f posOffset ( 0.0f , 0.0f , 0.0f ) ;
const float delTime = 0.001f * ( timeStamp_ms - _last_body_odm_update_ms ) ;
2017-04-30 21:09:57 -03:00
_last_body_odm_update_ms = timeStamp_ms ;
timeStamp_ms - = ( timeStamp_ms - _last_body_odm_update_ms ) / 2 ; // correct for first order hold average delay
2018-07-19 05:09:17 -03:00
Vector3f delAng = _ins . get_gyro ( ) ;
2017-04-30 21:09:57 -03:00
delAng * = delTime ;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn ;
Tbn . from_euler ( radians ( fdm . rollDeg ) , radians ( fdm . pitchDeg ) , radians ( fdm . yawDeg ) ) ;
2017-12-02 09:02:37 -04:00
const Vector3f earth_vel ( fdm . speedN , fdm . speedE , fdm . speedD ) ;
2017-12-03 06:10:19 -04:00
const Vector3f delPos = Tbn . transposed ( ) * ( earth_vel * delTime ) ;
2017-04-30 21:09:57 -03:00
// write to EKF
EKF3 . writeBodyFrameOdom ( quality , delPos , delAng , delTime , timeStamp_ms , posOffset ) ;
}
2015-11-20 04:34:30 -04:00
}
}
# 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 )
{
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2014-01-01 21:15:58 -04:00
AP_AHRS_DCM : : reset ( recover_eulers ) ;
2015-10-20 18:58:30 -03:00
_dcm_attitude ( roll , pitch , yaw ) ;
2016-12-19 00:38:44 -04:00
if ( _ekf2_started ) {
_ekf2_started = EKF2 . InitialiseFilter ( ) ;
2014-01-01 22:05:28 -04:00
}
2016-12-19 00:38:44 -04:00
if ( _ekf3_started ) {
_ekf3_started = EKF3 . InitialiseFilter ( ) ;
2016-07-14 02:08:42 -03: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 )
{
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2014-01-01 21:15:58 -04:00
AP_AHRS_DCM : : reset_attitude ( _roll , _pitch , _yaw ) ;
2015-10-20 18:58:30 -03:00
_dcm_attitude ( roll , pitch , yaw ) ;
2016-12-19 00:38:44 -04:00
if ( _ekf2_started ) {
_ekf2_started = EKF2 . InitialiseFilter ( ) ;
2014-01-01 22:05:28 -04:00
}
2016-12-19 00:38:44 -04:00
if ( _ekf3_started ) {
_ekf3_started = EKF3 . InitialiseFilter ( ) ;
2016-07-14 02:08:42 -03: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-06-28 04:36:12 -03:00
case EKF_TYPE_NONE :
2017-01-21 22:39:54 -04:00
return AP_AHRS_DCM : : get_position ( loc ) ;
2016-06-28 04:36:12 -03:00
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2017-05-29 22:38:49 -03:00
if ( EKF2 . getLLH ( loc ) & & EKF2 . getPosD ( - 1 , ned_pos . z ) & & EKF2 . getOriginLLH ( - 1 , 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-07-14 02:08:42 -03:00
case EKF_TYPE3 :
2017-05-29 22:38:49 -03:00
if ( EKF3 . getLLH ( loc ) & & EKF3 . getPosD ( - 1 , ned_pos . z ) & & EKF3 . getOriginLLH ( - 1 , origin ) ) {
2016-07-14 02:08:42 -03:00
// fixup altitude using relative position from EKF origin
loc . alt = origin . alt - ned_pos . z * 100 ;
return true ;
}
break ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
2018-06-06 21:01:02 -03:00
if ( _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 ;
}
break ;
2015-11-20 04:34:30 -04:00
}
# 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
2017-12-02 09:13:32 -04:00
Vector3f AP_AHRS_NavEKF : : wind_estimate ( void ) const
2014-01-01 21:15:58 -04:00
{
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 ;
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 :
EKF2 . getWind ( - 1 , wind ) ;
break ;
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 :
EKF3 . 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 ;
case EKF_TYPE2 :
return EKF2 . use_compass ( ) ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
return EKF3 . use_compass ( ) ;
2016-06-28 04:36:12 -03:00
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
2017-12-02 09:13:32 -04:00
bool AP_AHRS_NavEKF : : get_secondary_attitude ( Vector3f & eulers ) const
2014-01-01 22:47:40 -04:00
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2016-12-22 18:47:46 -04:00
case EKF_TYPE_NONE :
2014-01-01 22:47:40 -04:00
// EKF is secondary
2016-04-03 22:08:03 -03:00
EKF2 . getEulerAngles ( - 1 , eulers ) ;
2016-12-19 00:38:44 -04:00
return _ekf2_started ;
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
2016-06-28 04:36:12 -03:00
2015-09-22 22:40:25 -03:00
default :
2016-12-22 18:47:46 -04:00
// DCM is secondary
eulers = _dcm_attitude ;
return true ;
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 AP_AHRS_NavEKF : : get_secondary_quaternion ( Quaternion & quat ) const
2017-04-15 08:21:09 -03:00
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
// EKF is secondary
EKF2 . getQuaternion ( - 1 , quat ) ;
return _ekf2_started ;
case EKF_TYPE2 :
case EKF_TYPE3 :
default :
// DCM is secondary
quat . from_rotation_matrix ( AP_AHRS_DCM : : get_rotation_body_to_ned ( ) ) ;
return true ;
}
}
2014-01-01 22:47:40 -04:00
// return secondary position solution if available
2017-12-02 09:13:32 -04:00
bool AP_AHRS_NavEKF : : get_secondary_position ( struct Location & loc ) const
2014-01-01 22:47:40 -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 :
// EKF is secondary
2016-04-03 22:08:03 -03:00
EKF2 . getLLH ( loc ) ;
2016-12-19 00:38:44 -04:00
return _ekf2_started ;
2015-09-22 22:40:25 -03:00
case EKF_TYPE2 :
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
2016-06-28 04:36:12 -03:00
2015-09-22 22:40:25 -03:00
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-06-28 04:36:12 -03:00
case EKF_TYPE2 :
default :
EKF2 . getVelNED ( - 1 , vec ) ;
2015-09-22 22:40:25 -03:00
return Vector2f ( vec . x , vec . y ) ;
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
EKF3 . getVelNED ( - 1 , vec ) ;
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 : {
2018-06-06 21:01:02 -03:00
if ( _sitl ) {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
return Vector2f ( fdm . speedN , fdm . speedE ) ;
} else {
return AP_AHRS_DCM : : groundspeed_vector ( ) ;
}
2015-11-20 04:34:30 -04:00
}
# endif
2014-01-01 23:25:41 -04: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 AP_AHRS_NavEKF : : set_origin ( const Location & loc )
{
2017-12-03 06:10:19 -04:00
const bool ret2 = EKF2 . setOriginLLH ( loc ) ;
const bool ret3 = EKF3 . setOriginLLH ( loc ) ;
2017-04-19 03:28:14 -03:00
// return success if active EKF's origin was set
switch ( active_EKF_type ( ) ) {
case EKF_TYPE2 :
return ret2 ;
case EKF_TYPE3 :
return ret3 ;
2017-05-02 06:36:15 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2018-06-06 21:01:02 -03:00
case EKF_TYPE_SITL :
if ( _sitl ) {
struct SITL : : sitl_fdm & fdm = _sitl - > state ;
fdm . home = loc ;
return true ;
} else {
return false ;
}
2017-05-02 06:36:15 -03:00
# endif
2017-04-19 03:28:14 -03:00
default :
return false ;
}
}
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 ( ) ) {
2016-12-22 18:47:46 -04:00
case EKF_TYPE_NONE :
return false ;
2015-09-22 22:40:25 -03:00
2016-12-22 18:47:46 -04:00
case EKF_TYPE2 :
default :
EKF2 . getVelNED ( - 1 , vec ) ;
return true ;
2015-09-22 22:40:25 -03:00
2016-12-22 18:47:46 -04:00
case EKF_TYPE3 :
EKF3 . getVelNED ( - 1 , vec ) ;
return true ;
2016-07-14 02:08:42 -03:00
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2016-12-22 18:47:46 -04:00
case EKF_TYPE_SITL :
2018-06-06 21:01:02 -03:00
if ( ! _sitl ) {
return false ;
}
2016-12-22 18:47:46 -04:00
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
vec = Vector3f ( fdm . speedN , fdm . speedE , fdm . speedD ) ;
return true ;
2015-11-20 04:34:30 -04:00
# 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 ( ) ) {
2016-12-22 18:47:46 -04:00
case EKF_TYPE_NONE :
return false ;
2016-01-04 19:57:17 -04:00
2016-12-22 18:47:46 -04:00
case EKF_TYPE2 :
default :
EKF2 . getMagNED ( - 1 , vec ) ;
return true ;
2016-01-04 19:57:17 -04:00
2016-12-22 18:47:46 -04:00
case EKF_TYPE3 :
EKF3 . getMagNED ( - 1 , vec ) ;
return true ;
2016-01-04 19:57:17 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2016-12-22 18:47:46 -04:00
case EKF_TYPE_SITL :
return false ;
2016-01-04 19:57:17 -04:00
# 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 ( ) ) {
2016-12-22 18:47:46 -04:00
case EKF_TYPE_NONE :
return false ;
2016-01-04 19:57:17 -04:00
2016-12-22 18:47:46 -04:00
case EKF_TYPE2 :
default :
EKF2 . getMagXYZ ( - 1 , vec ) ;
return true ;
2016-01-04 19:57:17 -04:00
2016-12-22 18:47:46 -04:00
case EKF_TYPE3 :
EKF3 . getMagXYZ ( - 1 , vec ) ;
return true ;
2016-01-04 19:57:17 -04:00
2016-12-22 18:47:46 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return false ;
# endif
2016-01-04 19:57:17 -04:00
}
}
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.
2017-12-02 09:13:32 -04:00
bool AP_AHRS_NavEKF : : get_vert_pos_rate ( float & velocity ) const
2015-10-12 06:50:01 -03:00
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
2016-06-28 04:36:12 -03:00
case EKF_TYPE2 :
default :
velocity = EKF2 . getPosDownDerivative ( - 1 ) ;
2015-10-12 06:50:01 -03:00
return true ;
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
velocity = EKF3 . getPosDownDerivative ( - 1 ) ;
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2018-06-06 21:01:02 -03:00
case EKF_TYPE_SITL :
if ( _sitl ) {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
velocity = fdm . speedD ;
return true ;
} else {
return false ;
}
2015-11-20 04:34:30 -04:00
# 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
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 ) ;
2016-07-14 02:08:42 -03:00
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 :
return EKF3 . getHAGL ( height ) ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
2018-06-06 21:01:02 -03:00
if ( ! _sitl ) {
return false ;
}
2015-11-20 04:34:30 -04:00
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
}
}
2017-01-30 15:06:13 -04:00
// return a relative ground position to the origin in meters
// North/East/Down order.
bool AP_AHRS_NavEKF : : get_relative_position_NED_origin ( 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-06-28 04:36:12 -03:00
case EKF_TYPE2 :
default : {
2016-07-09 08:36:09 -03:00
Vector2f posNE ;
float posD ;
2016-06-28 04:36:12 -03:00
if ( EKF2 . getPosNE ( - 1 , posNE ) & & EKF2 . getPosD ( - 1 , posD ) ) {
2016-07-19 21:38:26 -03:00
// 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-09-22 22:40:25 -03:00
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 : {
Vector2f posNE ;
float posD ;
2016-12-19 22:42:54 -04:00
if ( EKF3 . getPosNE ( - 1 , posNE ) & & EKF3 . getPosD ( - 1 , posD ) ) {
// position is valid
vec . x = posNE . x ;
vec . y = posNE . y ;
vec . z = posD ;
return true ;
}
return false ;
2016-07-19 21:38:26 -03:00
}
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
2018-06-06 21:01:02 -03:00
if ( ! _sitl ) {
return false ;
}
2015-11-20 04:34:30 -04:00
Location loc ;
get_position ( loc ) ;
2017-12-03 06:10:19 -04:00
const Vector2f diff2d = location_diff ( get_home ( ) , loc ) ;
2015-11-20 04:34:30 -04:00
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
}
}
2017-01-30 15:06:13 -04:00
// return a relative ground position to the home in meters
// North/East/Down order.
bool AP_AHRS_NavEKF : : get_relative_position_NED_home ( Vector3f & vec ) const
{
Location originLLH ;
Vector3f originNED ;
if ( ! get_relative_position_NED_origin ( originNED ) | |
! get_origin ( originLLH ) ) {
return false ;
}
2017-12-03 06:10:19 -04:00
const Vector3f offset = location_3d_diff_NED ( originLLH , _home ) ;
2017-01-30 15:06:13 -04:00
2017-08-16 23:59:06 -03:00
vec . x = originNED . x - offset . x ;
vec . y = originNED . y - offset . y ;
2017-01-30 15:06:13 -04:00
vec . z = originNED . z - offset . z ;
return true ;
}
// write a relative ground position estimate to the origin in meters, North/East order
2016-07-11 20:36:41 -03:00
// return true if estimate is valid
2017-01-30 15:06:13 -04:00
bool AP_AHRS_NavEKF : : get_relative_position_NE_origin ( Vector2f & posNE ) const
2016-07-09 08:36:09 -03:00
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
case EKF_TYPE2 :
default : {
bool position_is_valid = EKF2 . getPosNE ( - 1 , posNE ) ;
return position_is_valid ;
}
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 : {
bool position_is_valid = EKF3 . getPosNE ( - 1 , posNE ) ;
return position_is_valid ;
}
2016-07-09 08:36:09 -03:00
# 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
}
}
2017-01-30 15:06:13 -04:00
// return a relative ground position to the home in meters
// North/East order.
bool AP_AHRS_NavEKF : : get_relative_position_NE_home ( Vector2f & posNE ) const
{
Location originLLH ;
Vector2f originNE ;
if ( ! get_relative_position_NE_origin ( originNE ) | |
! get_origin ( originLLH ) ) {
return false ;
}
2017-12-03 06:10:19 -04:00
const Vector2f offset = location_diff ( originLLH , _home ) ;
2017-01-30 15:06:13 -04:00
2017-08-16 23:59:06 -03:00
posNE . x = originNE . x - offset . x ;
posNE . y = originNE . y - offset . y ;
2017-01-30 15:06:13 -04:00
return true ;
}
// write a relative ground position estimate to the origin in meters, North/East order
// write a relative ground position to the origin in meters, Down
2016-07-09 08:36:09 -03:00
// return true if the estimate is valid
2017-01-30 15:06:13 -04:00
bool AP_AHRS_NavEKF : : get_relative_position_D_origin ( float & posD ) const
2016-07-09 08:36:09 -03:00
{
switch ( active_EKF_type ( ) ) {
case EKF_TYPE_NONE :
return false ;
case EKF_TYPE2 :
default : {
bool position_is_valid = EKF2 . getPosD ( - 1 , posD ) ;
return position_is_valid ;
}
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 : {
bool position_is_valid = EKF3 . getPosD ( - 1 , posD ) ;
return position_is_valid ;
}
2016-07-09 08:36:09 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL : {
2018-06-06 21:01:02 -03:00
if ( ! _sitl ) {
return false ;
}
2016-07-09 08:36:09 -03:00
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
posD = - ( fdm . altitude - get_home ( ) . alt * 0.01f ) ;
return true ;
}
# endif
}
}
2017-01-30 15:06:13 -04:00
// write a relative ground position to home in meters, Down
// will use the barometer if the EKF isn't available
void AP_AHRS_NavEKF : : get_relative_position_D_home ( float & posD ) const
{
Location originLLH ;
float originD ;
if ( ! get_relative_position_D_origin ( originD ) | |
! get_origin ( originLLH ) ) {
2018-03-05 16:34:26 -04:00
posD = - AP : : baro ( ) . get_altitude ( ) ;
2017-01-30 15:06:13 -04:00
return ;
}
posD = originD - ( ( originLLH . alt - _home . alt ) * 0.01f ) ;
return ;
}
2015-09-22 22:40:25 -03:00
/*
2016-06-28 04:36:12 -03:00
canonicalise _ekf_type , forcing it to be 0 , 2 or 3
type 1 has been deprecated
2015-09-22 22:40:25 -03:00
*/
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 ;
2016-12-20 19:52:12 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if ( type = = EKF_TYPE_SITL ) {
return type ;
}
# endif
2016-06-28 04:36:12 -03:00
if ( ( always_use_EKF ( ) & & ( type = = 0 ) ) | | ( type = = 1 ) | | ( type > 3 ) ) {
2016-04-03 22:08:03 -03:00
type = 2 ;
2015-11-20 04:34:30 -04:00
}
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 ;
case 2 : {
// do we have an EKF2 yet?
2016-12-19 00:38:44 -04:00
if ( ! _ekf2_started ) {
2015-09-22 22:40:25 -03:00
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
2016-07-14 02:08:42 -03:00
case 3 : {
// do we have an EKF3 yet?
2016-12-19 00:38:44 -04:00
if ( ! _ekf3_started ) {
2016-07-14 02:08:42 -03:00
return EKF_TYPE_NONE ;
}
if ( always_use_EKF ( ) ) {
uint16_t ekf3_faults ;
EKF3 . getFilterFaults ( - 1 , ekf3_faults ) ;
if ( ekf3_faults = = 0 ) {
ret = EKF_TYPE3 ;
}
} else if ( EKF3 . healthy ( ) ) {
ret = EKF_TYPE3 ;
}
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
2016-09-01 22:11:56 -03:00
DCM is very robust . Note that we also check the filter status
when fly_forward is false and we are disarmed . This is to ensure
that the arming checks do wait for good GPS position on fixed
wing and rover
2016-03-04 17:52:40 -04:00
*/
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 ) & &
2016-09-01 22:11:56 -03:00
( _flags . fly_forward | | ! hal . util - > get_soft_armed ( ) ) ) {
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 ) ;
2016-07-14 02:08:42 -03:00
} else if ( ret = = EKF_TYPE3 ) {
EKF3 . 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 ) ;
2016-04-03 22:08:03 -03:00
# endif
2015-09-22 22:40:25 -03:00
}
2017-12-01 19:50:46 -04:00
if ( hal . util - > get_soft_armed ( ) & & ! filt_state . flags . using_gps & & AP : : gps ( ) . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
2015-05-19 02:15:44 -03:00
// 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 | |
2017-06-06 06:29:23 -03:00
( ! filt_state . flags . horiz_pos_abs & & ! filt_state . flags . horiz_pos_rel ) ) {
2016-05-26 21:18:53 -03:00
if ( ( ! _compass | | ! _compass - > use_for_yaw ( ) ) & &
2017-12-01 19:50:46 -04:00
AP : : gps ( ) . status ( ) > = AP_GPS : : GPS_OK_FIX_3D & &
AP : : gps ( ) . ground_speed ( ) < 2 ) {
2016-05-26 21:18:53 -03:00
/*
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
*/
2018-07-13 21:11:23 -03:00
if ( filt_state . flags . gps_quality_good ) {
2016-05-26 21:18:53 -03:00
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 ( ) ;
case 2 : {
2016-12-19 00:38:44 -04:00
bool ret = _ekf2_started & & EKF2 . healthy ( ) ;
2015-09-22 22:40:25 -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_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
2016-07-14 02:08:42 -03:00
case 3 : {
2016-12-19 00:38:44 -04:00
bool ret = _ekf3_started & & EKF3 . healthy ( ) ;
2016-07-14 02:08:42 -03:00
if ( ! ret ) {
return false ;
}
if ( ( _vehicle_class = = AHRS_VEHICLE_FIXED_WING | |
_vehicle_class = = AHRS_VEHICLE_GROUND ) & &
active_EKF_type ( ) ! = EKF_TYPE3 ) {
// 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 ;
2016-06-28 04:36:12 -03:00
case 2 :
default :
2015-09-22 22:40:25 -03:00
// initialisation complete 10sec after ekf has started
2016-12-19 00:38:44 -04:00
return ( _ekf2_started & & ( AP_HAL : : millis ( ) - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS ) ) ;
2015-09-22 22:40:25 -03:00
2016-07-14 02:08:42 -03:00
case 3 :
// initialisation complete 10sec after ekf has started
2016-12-19 00:38:44 -04:00
return ( _ekf3_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-06-28 04:36:12 -03:00
case EKF_TYPE2 :
default :
EKF2 . getFilterStatus ( - 1 , status ) ;
2015-10-15 02:10:03 -03:00
return true ;
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
EKF3 . getFilterStatus ( - 1 , status ) ;
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
2016-10-27 01:45:24 -03:00
void AP_AHRS_NavEKF : : writeOptFlowMeas ( uint8_t & rawFlowQuality , Vector2f & rawFlowRates , Vector2f & rawGyroRates , uint32_t & msecFlowMeas , const Vector3f & posOffset )
2014-08-06 21:31:24 -03:00
{
2016-10-11 18:16:10 -03:00
EKF2 . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas , posOffset ) ;
2016-07-14 02:08:42 -03:00
EKF3 . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas , posOffset ) ;
2014-08-06 21:31:24 -03:00
}
2014-11-12 14:49:15 -04:00
2017-03-21 18:13:54 -03:00
// write body frame odometry measurements to the EKF
void AP_AHRS_NavEKF : : writeBodyFrameOdom ( float quality , const Vector3f & delPos , const Vector3f & delAng , float delTime , uint32_t timeStamp_ms , const Vector3f & posOffset )
{
EKF3 . writeBodyFrameOdom ( quality , delPos , delAng , delTime , timeStamp_ms , posOffset ) ;
}
2018-03-07 20:44:53 -04:00
// Write position and quaternion data from an external navigation system
void AP_AHRS_NavEKF : : writeExtNavData ( const Vector3f & sensOffset , const Vector3f & pos , const Quaternion & quat , float posErr , float angErr , uint32_t timeStamp_ms , uint32_t resetTime_ms )
{
EKF2 . writeExtNavData ( sensOffset , pos , quat , posErr , angErr , timeStamp_ms , resetTime_ms ) ;
}
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 :
2016-07-14 02:08:42 -03:00
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
return EKF2 . setInhibitGPS ( ) ;
2015-11-20 04:34:30 -04:00
2016-06-28 04:36:12 -03:00
case 3 :
return EKF3 . 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
2017-12-02 09:13:32 -04:00
void AP_AHRS_NavEKF : : getEkfControlLimits ( float & ekfGndSpdLimit , float & ekfNavVelGainScaler ) const
2014-11-15 19:36:33 -04:00
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
case 2 :
EKF2 . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
break ;
2015-11-20 04:34:30 -04:00
2016-06-28 04:36:12 -03:00
case 3 :
EKF3 . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2015-11-20 04:34:30 -04:00
break ;
2016-06-28 04:36:12 -03:00
2016-12-29 03:43:32 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
// same as EKF2 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
2017-12-02 09:13:32 -04:00
bool AP_AHRS_NavEKF : : getMagOffsets ( uint8_t mag_idx , Vector3f & magOffsets ) const
2015-03-16 18:29:11 -03:00
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
case 0 :
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
2016-06-28 04:36:12 -03:00
case 3 :
return EKF3 . 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
}
2016-08-11 16:58:02 -03:00
// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF : : getCorrectedDeltaVelocityNED ( Vector3f & ret , float & dt ) const
{
2017-12-03 06:10:19 -04:00
const EKF_TYPE type = active_EKF_type ( ) ;
2017-03-04 02:38:57 -04:00
if ( type = = EKF_TYPE2 | | type = = EKF_TYPE3 ) {
int8_t imu_idx = 0 ;
2016-06-28 04:36:12 -03:00
Vector3f accel_bias ;
2017-03-04 02:38:57 -04:00
if ( type = = EKF_TYPE2 ) {
2016-06-28 04:36:12 -03:00
accel_bias . zero ( ) ;
imu_idx = EKF2 . getPrimaryCoreIMUIndex ( ) ;
EKF2 . getAccelZBias ( - 1 , accel_bias . z ) ;
2017-03-04 02:38:57 -04:00
} else if ( type = = EKF_TYPE3 ) {
2016-06-28 04:36:12 -03:00
imu_idx = EKF3 . getPrimaryCoreIMUIndex ( ) ;
EKF3 . getAccelBias ( - 1 , accel_bias ) ;
}
2017-03-04 02:38:57 -04:00
if ( imu_idx = = - 1 ) {
// should never happen, call parent implementation in this scenario
AP_AHRS : : getCorrectedDeltaVelocityNED ( ret , dt ) ;
return ;
}
2016-08-11 16:58:02 -03:00
ret . zero ( ) ;
2018-03-10 05:35:03 -04:00
const AP_InertialSensor & _ins = AP : : ins ( ) ;
2017-03-04 02:38:57 -04:00
_ins . get_delta_velocity ( ( uint8_t ) imu_idx , ret ) ;
dt = _ins . get_delta_velocity_dt ( ( uint8_t ) imu_idx ) ;
2016-06-28 04:36:12 -03:00
ret - = accel_bias * dt ;
2016-08-11 16:58:02 -03:00
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * ret ;
ret . z + = GRAVITY_MSS * dt ;
} else {
AP_AHRS : : getCorrectedDeltaVelocityNED ( ret , dt ) ;
}
}
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 ;
2016-06-28 04:36:12 -03:00
2015-09-22 22:40:25 -03:00
case 2 :
2016-06-28 04:36:12 -03:00
default :
2015-10-30 01:00:22 -03:00
return EKF2 . prearm_failure_reason ( ) ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case 3 :
return EKF3 . prearm_failure_reason ( ) ;
2016-06-28 04:36:12 -03:00
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 ( ) ) {
2016-06-28 04:36:12 -03:00
2015-09-23 04:46:51 -03:00
case 2 :
2016-06-28 04:36:12 -03:00
default :
2015-09-23 04:46:51 -03:00
return EKF2 . getLastYawResetAngle ( yawAng ) ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case 3 :
return EKF3 . getLastYawResetAngle ( yawAng ) ;
2016-06-28 04:36:12 -03:00
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 ( ) ) {
2016-06-28 04:36:12 -03:00
2015-10-29 02:18:04 -03:00
case 2 :
2016-06-28 04:36:12 -03:00
default :
2015-10-29 02:18:04 -03:00
return EKF2 . getLastPosNorthEastReset ( pos ) ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case 3 :
return EKF3 . getLastPosNorthEastReset ( pos ) ;
2016-06-28 04:36:12 -03:00
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 ( ) ) {
2016-06-28 04:36:12 -03:00
2015-10-29 02:18:04 -03:00
case 2 :
2016-06-28 04:36:12 -03:00
default :
2015-10-29 02:18:04 -03:00
return EKF2 . getLastVelNorthEastReset ( vel ) ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case 3 :
return EKF3 . getLastVelNorthEastReset ( vel ) ;
2016-06-28 04:36:12 -03:00
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 ;
}
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
uint32_t AP_AHRS_NavEKF : : getLastPosDownReset ( float & posDelta ) const
{
switch ( ekf_type ( ) ) {
2016-06-28 04:36:12 -03:00
case EKF_TYPE2 :
2016-11-22 01:28:14 -04:00
return EKF2 . getLastPosDownReset ( posDelta ) ;
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 :
return EKF3 . getLastPosDownReset ( posDelta ) ;
2016-11-22 01:28:14 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
return 0 ;
# endif
}
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 )
{
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2015-09-23 04:46:51 -03:00
switch ( ekf_type ( ) ) {
2016-06-28 04:36:12 -03:00
2015-09-23 04:46:51 -03:00
case 2 :
2016-06-28 04:36:12 -03:00
default : {
2016-07-14 02:08:42 -03:00
EKF3 . resetHeightDatum ( ) ;
2015-09-23 04:46:51 -03:00
return EKF2 . resetHeightDatum ( ) ;
2016-06-28 04:36:12 -03:00
}
case 3 : {
2016-07-14 02:08:42 -03:00
EKF2 . resetHeightDatum ( ) ;
return EKF3 . resetHeightDatum ( ) ;
2016-06-28 04:36:12 -03:00
}
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
2017-12-02 09:13:32 -04:00
void AP_AHRS_NavEKF : : send_ekf_status_report ( mavlink_channel_t chan ) const
2015-09-28 21:58:54 -03:00
{
2017-09-01 04:43:03 -03:00
switch ( ekf_type ( ) ) {
2016-06-28 04:36:12 -03:00
case EKF_TYPE_NONE :
// send zero status report
2018-04-30 02:39:43 -03:00
mavlink_msg_ekf_status_report_send ( chan , 0 , 0 , 0 , 0 , 0 , 0 , 0 ) ;
2016-06-28 04:36:12 -03:00
break ;
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
2018-04-30 02:39:43 -03:00
mavlink_msg_ekf_status_report_send ( chan , 0 , 0 , 0 , 0 , 0 , 0 , 0 ) ;
2016-06-19 09:02:25 -03:00
break ;
# endif
2015-09-28 21:58:54 -03:00
case EKF_TYPE2 :
return EKF2 . send_status_report ( chan ) ;
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 :
return EKF3 . send_status_report ( chan ) ;
}
2015-09-28 21:58:54 -03:00
}
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-06-28 04:36:12 -03:00
case EKF_TYPE2 :
default :
2017-05-29 22:38:49 -03:00
if ( ! EKF2 . getOriginLLH ( - 1 , 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-07-14 02:08:42 -03:00
case EKF_TYPE3 :
2017-05-29 22:38:49 -03:00
if ( ! EKF3 . getOriginLLH ( - 1 , ret ) ) {
2016-07-14 02:08:42 -03:00
return false ;
}
return true ;
2015-11-20 04:34:30 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
2018-06-06 21:01:02 -03:00
if ( ! _sitl ) {
return false ;
}
2017-05-02 06:36:15 -03:00
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
ret = fdm . home ;
return true ;
2015-11-20 04:34:30 -04:00
# 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
2017-12-07 00:28:11 -04:00
// it will return false when no limiting is required
2015-10-12 06:50:01 -03:00
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
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
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 :
return EKF3 . 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
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
2016-06-28 04:36:12 -03:00
case EKF_TYPE3 :
return EKF3 . 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-06-28 04:36:12 -03:00
case EKF_TYPE2 :
default :
2015-10-17 02:49:50 -03:00
// use EKF to get variance
2016-06-28 04:36:12 -03:00
EKF2 . getVariances ( - 1 , velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
2015-10-17 02:49:50 -03:00
return true ;
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
// use EKF to get variance
EKF3 . getVariances ( - 1 , velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
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 ( ) ) {
case EKF_TYPE2 :
2016-06-28 04:36:12 -03:00
default :
2015-12-22 15:18:03 -04:00
EKF2 . setTakeoffExpected ( val ) ;
break ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
EKF3 . setTakeoffExpected ( val ) ;
break ;
2016-06-28 04:36:12 -03:00
2015-12-22 15:18:03 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
break ;
# endif
}
}
void AP_AHRS_NavEKF : : setTouchdownExpected ( bool val )
{
switch ( ekf_type ( ) ) {
case EKF_TYPE2 :
2016-06-28 04:36:12 -03:00
default :
2015-12-22 15:18:03 -04:00
EKF2 . setTouchdownExpected ( val ) ;
break ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case EKF_TYPE3 :
EKF3 . setTouchdownExpected ( val ) ;
break ;
2016-06-28 04:36:12 -03:00
2015-12-22 15:18:03 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL :
break ;
# endif
}
}
2017-12-02 09:13:32 -04:00
bool AP_AHRS_NavEKF : : getGpsGlitchStatus ( ) const
2016-01-26 17:28:21 -04:00
{
2016-12-30 17:53:16 -04:00
nav_filter_status ekf_status { } ;
if ( ! get_filter_status ( ekf_status ) ) {
return false ;
}
2016-01-26 17:28:21 -04:00
return ekf_status . flags . gps_glitching ;
}
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 ( ) ;
2016-06-28 04:36:12 -03:00
2016-07-14 02:08:42 -03:00
case 3 :
return EKF3 . have_ekf_logging ( ) ;
2016-06-28 04:36:12 -03:00
2016-05-04 20:24:19 -03:00
default :
break ;
}
2016-05-04 01:02:12 -03:00
return false ;
}
2017-03-02 07:33:13 -04:00
// get the index of the current primary IMU
2016-09-03 04:54:36 -03:00
uint8_t AP_AHRS_NavEKF : : get_primary_IMU_index ( ) const
2016-09-03 04:12:39 -03:00
{
int8_t imu = - 1 ;
switch ( ekf_type ( ) ) {
case 2 :
// let EKF2 choose primary IMU
imu = EKF2 . getPrimaryCoreIMUIndex ( ) ;
break ;
2016-06-28 04:36:12 -03:00
case 3 :
// let EKF2 choose primary IMU
imu = EKF3 . getPrimaryCoreIMUIndex ( ) ;
break ;
2016-09-03 04:12:39 -03:00
default :
break ;
}
if ( imu = = - 1 ) {
2018-03-10 05:35:03 -04:00
imu = AP : : ins ( ) . get_primary_accel ( ) ;
2016-09-03 04:12:39 -03:00
}
2016-09-03 04:54:36 -03:00
return imu ;
}
// get earth-frame accel vector for primary IMU
const Vector3f & AP_AHRS_NavEKF : : get_accel_ef ( ) const
{
return get_accel_ef ( get_primary_accel_index ( ) ) ;
}
// get the index of the current primary accelerometer sensor
uint8_t AP_AHRS_NavEKF : : get_primary_accel_index ( void ) const
{
2016-06-28 04:36:12 -03:00
if ( ekf_type ( ) ! = 0 ) {
2016-09-03 04:54:36 -03:00
return get_primary_IMU_index ( ) ;
}
2018-03-10 05:35:03 -04:00
return AP : : ins ( ) . get_primary_accel ( ) ;
2016-09-03 04:54:36 -03:00
}
// get the index of the current primary gyro sensor
uint8_t AP_AHRS_NavEKF : : get_primary_gyro_index ( void ) const
{
2016-06-28 04:36:12 -03:00
if ( ekf_type ( ) ! = 0 ) {
2016-09-03 04:54:36 -03:00
return get_primary_IMU_index ( ) ;
}
2018-03-10 05:35:03 -04:00
return AP : : ins ( ) . get_primary_gyro ( ) ;
2016-09-03 04:12:39 -03:00
}
2018-04-06 09:17:04 -03:00
AP_AHRS_NavEKF & AP : : ahrs_navekf ( )
{
return static_cast < AP_AHRS_NavEKF & > ( * AP_AHRS : : get_singleton ( ) ) ;
}
2014-01-01 21:15:58 -04:00
# endif // AP_AHRS_NAVEKF_AVAILABLE
2014-02-14 18:24:12 -04:00