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"
2020-10-16 01:37:25 -03:00
# include <AP_BoardConfig/AP_BoardConfig.h>
2022-02-25 01:06:27 -04:00
# include <AP_ExternalAHRS/AP_ExternalAHRS.h>
2016-07-07 05:26:53 -03:00
# include <AP_Module/AP_Module.h>
2019-06-14 00:15:53 -03:00
# include <AP_GPS/AP_GPS.h>
2019-06-26 23:37:09 -03:00
# include <AP_Baro/AP_Baro.h>
2022-02-25 01:06:27 -04:00
# include <AP_Compass/AP_Compass.h>
2020-07-29 01:16:26 -03:00
# include <AP_InternalError/AP_InternalError.h>
2020-11-05 19:30:50 -04:00
# include <AP_Logger/AP_Logger.h>
2020-07-29 01:16:26 -03:00
# include <AP_Notify/AP_Notify.h>
# include <AP_Vehicle/AP_Vehicle_Type.h>
2020-12-30 18:42:44 -04:00
# include <GCS_MAVLink/GCS.h>
2021-08-26 01:34:08 -03:00
# include <AP_InertialSensor/AP_InertialSensor.h>
2021-11-05 13:13:45 -03:00
# include <AP_CustomRotations/AP_CustomRotations.h>
2014-01-01 21:15:58 -04:00
2019-02-19 23:52:55 -04:00
# define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
# define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
2021-07-21 08:32:38 -03:00
# ifndef HAL_AHRS_EKF_TYPE_DEFAULT
# define HAL_AHRS_EKF_TYPE_DEFAULT 3
# endif
// table of user settable parameters
const AP_Param : : GroupInfo AP_AHRS : : var_info [ ] = {
// index 0 and 1 are for old parameters that are no longer not used
// @Param: GPS_GAIN
// @DisplayName: AHRS GPS gain
// @Description: This controls how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0.
// @Range: 0.0 1.0
// @Increment: .01
// @User: Advanced
AP_GROUPINFO ( " GPS_GAIN " , 2 , AP_AHRS , gps_gain , 1.0f ) ,
// @Param: GPS_USE
2021-08-24 05:05:05 -03:00
// @DisplayName: AHRS use GPS for DCM navigation and position-down
// @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS according to its own parameters. A value of 2 means to use GPS for height as well as position - both in DCM estimation and when determining altitude-above-home.
2021-07-21 08:32:38 -03:00
// @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
// @User: Advanced
AP_GROUPINFO ( " GPS_USE " , 3 , AP_AHRS , _gps_use , float ( GPSUse : : Enable ) ) ,
// @Param: YAW_P
// @DisplayName: Yaw P
// @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly.
// @Range: 0.1 0.4
// @Increment: .01
// @User: Advanced
AP_GROUPINFO ( " YAW_P " , 4 , AP_AHRS , _kp_yaw , 0.2f ) ,
// @Param: RP_P
// @DisplayName: AHRS RP_P
// @Description: This controls how fast the accelerometers correct the attitude
// @Range: 0.1 0.4
// @Increment: .01
// @User: Advanced
AP_GROUPINFO ( " RP_P " , 5 , AP_AHRS , _kp , 0.2f ) ,
// @Param: WIND_MAX
// @DisplayName: Maximum wind
// @Description: This sets the maximum allowable difference between ground speed and airspeed. This allows the plane to cope with a failing airspeed sensor. A value of zero means to use the airspeed as is. See ARSPD_OPTIONS and ARSPD_MAX_WIND to disable airspeed sensors.
// @Range: 0 127
// @Units: m/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " WIND_MAX " , 6 , AP_AHRS , _wind_max , 0.0f ) ,
// NOTE: 7 was BARO_USE
// @Param: TRIM_X
// @DisplayName: AHRS Trim Roll
// @Description: Compensates for the roll angle difference between the control board and the frame. Positive values make the vehicle roll right.
// @Units: rad
// @Range: -0.1745 +0.1745
// @Increment: 0.01
// @User: Standard
// @Param: TRIM_Y
// @DisplayName: AHRS Trim Pitch
// @Description: Compensates for the pitch angle difference between the control board and the frame. Positive values make the vehicle pitch up/back.
// @Units: rad
// @Range: -0.1745 +0.1745
// @Increment: 0.01
// @User: Standard
// @Param: TRIM_Z
// @DisplayName: AHRS Trim Yaw
// @Description: Not Used
// @Units: rad
// @Range: -0.1745 +0.1745
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO ( " TRIM " , 8 , AP_AHRS , _trim , 0 ) ,
// @Param: ORIENTATION
// @DisplayName: Board Orientation
2022-07-20 17:47:27 -03:00
// @Description: Overall board orientation relative to the standard orientation for the board type. This rotates the IMU and compass readings to allow the board to be oriented in your vehicle at any 90 or 45 degree angle. The label for each option is specified in the order of rotations for that orientation. This option takes affect on next boot. After changing you will need to re-level your vehicle. Firmware versions 4.2 and prior can use a CUSTOM (100) rotation to set the AHRS_CUSTOM_ROLL/PIT/YAW angles for AHRS orientation. Later versions provide two general custom rotations which can be used, Custom 1 and Custom 2, with CUST_ROT1_ROLL/PIT/YAW or CUST_ROT2_ROLL/PIT/YAW angles.
2021-11-05 13:08:26 -03:00
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2
2021-07-21 08:32:38 -03:00
// @User: Advanced
AP_GROUPINFO ( " ORIENTATION " , 9 , AP_AHRS , _board_orientation , 0 ) ,
// @Param: COMP_BETA
// @DisplayName: AHRS Velocity Complementary Filter Beta Coefficient
// @Description: This controls the time constant for the cross-over frequency used to fuse AHRS (airspeed and heading) and GPS data to estimate ground velocity. Time constant is 0.1/beta. A larger time constant will use GPS data less and a small time constant will use air data less.
// @Range: 0.001 0.5
// @Increment: .01
// @User: Advanced
AP_GROUPINFO ( " COMP_BETA " , 10 , AP_AHRS , beta , 0.1f ) ,
// @Param: GPS_MINSATS
// @DisplayName: AHRS GPS Minimum satellites
// @Description: Minimum number of satellites visible to use GPS for velocity based corrections attitude correction. This defaults to 6, which is about the point at which the velocity numbers from a GPS become too unreliable for accurate correction of the accelerometers.
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " GPS_MINSATS " , 11 , AP_AHRS , _gps_minsats , 6 ) ,
// NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
// of 1 was found to be the best choice
// 13 was the old EKF_USE
// @Param: EKF_TYPE
// @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
// @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
// @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3,11:ExternalAHRS
// @User: Advanced
AP_GROUPINFO ( " EKF_TYPE " , 14 , AP_AHRS , _ekf_type , HAL_AHRS_EKF_TYPE_DEFAULT ) ,
// @Param: CUSTOM_ROLL
// @DisplayName: Board orientation roll offset
// @Description: Autopilot mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
// @Range: -180 180
// @Units: deg
// @Increment: 1
// @User: Advanced
2021-11-05 13:13:45 -03:00
2021-11-05 13:08:26 -03:00
// index 15
2021-07-21 08:32:38 -03:00
// @Param: CUSTOM_PIT
// @DisplayName: Board orientation pitch offset
// @Description: Autopilot mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
// @Range: -180 180
// @Units: deg
// @Increment: 1
// @User: Advanced
2021-11-05 13:13:45 -03:00
2021-11-05 13:08:26 -03:00
// index 16
2021-07-21 08:32:38 -03:00
// @Param: CUSTOM_YAW
// @DisplayName: Board orientation yaw offset
// @Description: Autopilot mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
// @Range: -180 180
// @Units: deg
// @Increment: 1
// @User: Advanced
2021-11-05 13:13:45 -03:00
2021-11-05 13:08:26 -03:00
// index 17
2021-07-21 08:32:38 -03:00
AP_GROUPEND
} ;
2014-01-01 22:05:28 -04:00
extern const AP_HAL : : HAL & hal ;
2015-11-20 04:34:30 -04:00
// constructor
2021-07-20 10:04:20 -03:00
AP_AHRS : : AP_AHRS ( uint8_t flags ) :
2016-03-04 17:52:40 -04:00
_ekf_flags ( flags )
2015-11-20 04:34:30 -04:00
{
2021-07-20 10:04:20 -03:00
_singleton = this ;
// load default values from var_info table
AP_Param : : setup_object_defaults ( this , var_info ) ;
2021-10-25 14:08:59 -03:00
# if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduSub)
2019-11-25 23:48:07 -04:00
// Copter and Sub force the use of EKF
2021-07-20 10:04:20 -03:00
_ekf_flags | = AP_AHRS : : FLAG_ALWAYS_USE_EKF ;
2019-11-25 23:48:07 -04:00
# endif
2015-12-01 00:19:45 -04:00
_dcm_matrix . identity ( ) ;
2021-07-31 03:24:05 -03:00
// initialise the controller-to-autopilot-body trim state:
_last_trim = _trim . get ( ) ;
2021-08-16 23:52:01 -03:00
_rotation_autopilot_body_to_vehicle_body . from_euler ( _last_trim . x , _last_trim . y , _last_trim . z ) ;
2021-07-31 03:24:05 -03:00
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body . transposed ( ) ;
2015-11-20 04:34:30 -04:00
}
2020-04-11 19:26:48 -03:00
// init sets up INS board orientation
2021-07-20 10:04:20 -03:00
void AP_AHRS : : init ( )
2020-04-11 19:26:48 -03:00
{
2020-10-16 01:37:25 -03:00
// EKF1 is no longer supported - handle case where it is selected
if ( _ekf_type . get ( ) = = 1 ) {
AP_BoardConfig : : config_error ( " EKF1 not available " ) ;
}
2020-04-11 19:26:48 -03:00
# if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
if ( _ekf_type . get ( ) = = 2 ) {
_ekf_type . set ( 3 ) ;
EKF3 . set_enable ( true ) ;
}
# elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE
if ( _ekf_type . get ( ) = = 3 ) {
_ekf_type . set ( 2 ) ;
EKF2 . set_enable ( true ) ;
}
# endif
2020-12-30 18:42:44 -04:00
last_active_ekf_type = ( EKFType ) _ekf_type . get ( ) ;
2021-08-18 07:20:46 -03:00
// init backends
dcm . init ( ) ;
2021-07-24 07:22:19 -03:00
# if HAL_NMEA_OUTPUT_ENABLED
_nmea_out = AP_NMEA_Output : : probe ( ) ;
# endif
2021-11-05 13:13:45 -03:00
# if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
// convert to new custom rotaton
// PARAMETER_CONVERSION - Added: Nov-2021
if ( _board_orientation = = ROTATION_CUSTOM_OLD ) {
_board_orientation . set_and_save ( ROTATION_CUSTOM_1 ) ;
AP_Param : : ConversionInfo info ;
if ( AP_Param : : find_top_level_key_by_pointer ( this , info . old_key ) ) {
info . type = AP_PARAM_FLOAT ;
float rpy [ 3 ] = { } ;
AP_Float rpy_param ;
for ( info . old_group_element = 15 ; info . old_group_element < = 17 ; info . old_group_element + + ) {
if ( AP_Param : : find_old_parameter ( & info , & rpy_param ) ) {
rpy [ info . old_group_element - 15 ] = rpy_param . get ( ) ;
}
}
AP : : custom_rotations ( ) . convert ( ROTATION_CUSTOM_1 , rpy [ 0 ] , rpy [ 1 ] , rpy [ 2 ] ) ;
}
}
# endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
2020-04-11 19:26:48 -03:00
}
2021-07-31 03:24:05 -03:00
// updates matrices responsible for rotating vectors from vehicle body
// frame to autopilot body frame from _trim variables
void AP_AHRS : : update_trim_rotation_matrices ( )
{
if ( _last_trim = = _trim . get ( ) ) {
// nothing to do
return ;
}
_last_trim = _trim . get ( ) ;
2021-08-16 23:52:01 -03:00
_rotation_autopilot_body_to_vehicle_body . from_euler ( _last_trim . x , _last_trim . y , _last_trim . z ) ;
2021-07-31 03:24:05 -03:00
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body . transposed ( ) ;
}
2014-01-01 21:15:58 -04:00
// return the smoothed gyro vector corrected for drift
2021-07-20 10:04:20 -03:00
const Vector3f & AP_AHRS : : get_gyro ( void ) const
2014-01-01 21:15:58 -04:00
{
2014-07-13 08:56:39 -03:00
return _gyro_estimate ;
2014-01-01 21:15:58 -04:00
}
2021-07-20 10:04:20 -03:00
const Matrix3f & AP_AHRS : : get_rotation_body_to_ned ( void ) const
2014-01-01 21:15:58 -04:00
{
2014-01-01 22:05:28 -04:00
return _dcm_matrix ;
2014-01-01 21:15:58 -04:00
}
2021-07-20 10:04:20 -03:00
const Vector3f & AP_AHRS : : get_gyro_drift ( void ) const
2014-01-01 21:15:58 -04:00
{
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
2021-07-20 10:04:20 -03:00
void AP_AHRS : : reset_gyro_drift ( void )
2014-10-28 08:22:48 -03:00
{
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
2021-08-18 07:20:46 -03:00
dcm . reset_gyro_drift ( ) ;
2014-10-28 08:22:48 -03:00
2014-10-28 22:20:43 -03:00
// reset the EKF gyro bias states
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2015-09-22 22:40:25 -03:00
EKF2 . resetGyroBias ( ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2016-07-14 02:08:42 -03:00
EKF3 . resetGyroBias ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2014-10-28 08:22:48 -03:00
}
2021-07-20 10:04:20 -03:00
void AP_AHRS : : update ( bool skip_ins_update )
2015-09-22 22:40:25 -03:00
{
2019-05-01 04:34:21 -03:00
// periodically checks to see if we should update the AHRS
// orientation (e.g. based on the AHRS_ORIENTATION parameter)
// allow for runtime change of orientation
// this makes initial config easier
update_orientation ( ) ;
2021-08-26 01:34:08 -03:00
if ( ! skip_ins_update ) {
// tell the IMU to grab some data
AP : : ins ( ) . update ( ) ;
}
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2021-07-24 20:08:28 -03:00
// see if we have to restore home after a watchdog reset:
if ( ! _checked_watchdog_home ) {
load_watchdog_home ( ) ;
_checked_watchdog_home = true ;
}
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 ( ) ;
2017-12-12 21:27:55 -04:00
2021-07-31 03:24:05 -03:00
// update autopilot-body-to-vehicle-body from _trim parameters:
update_trim_rotation_matrices ( ) ;
2021-08-26 01:34:08 -03:00
update_DCM ( ) ;
2018-06-06 21:01:02 -03:00
2021-08-11 03:58:36 -03:00
// update takeoff/touchdown flags
update_flags ( ) ;
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2017-12-12 21:27:55 -04:00
update_SITL ( ) ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
update_external ( ) ;
# 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
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2017-04-28 21:26:29 -03:00
update_EKF2 ( ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2017-04-28 21:26:29 -03:00
update_EKF3 ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-28 21:26:29 -03:00
} else {
// otherwise run EKF3 first
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2017-04-28 21:26:29 -03:00
update_EKF3 ( ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF2_AVAILABLE
2017-04-28 21:26:29 -03:00
update_EKF2 ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-28 21:26:29 -03:00
}
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
2021-08-26 01:34:08 -03:00
_view - > update ( ) ;
2017-02-11 04:15:34 -04:00
}
2019-03-04 14:16:06 -04:00
2021-07-22 08:54:35 -03:00
// update AOA and SSA
update_AOA_SSA ( ) ;
2021-07-24 07:22:19 -03:00
# if HAL_NMEA_OUTPUT_ENABLED
2019-03-04 14:16:06 -04:00
// update NMEA output
2021-07-24 07:22:19 -03:00
if ( _nmea_out ! = nullptr ) {
_nmea_out - > update ( ) ;
}
2019-03-04 14:16:06 -04:00
# endif
2020-12-30 18:42:44 -04:00
EKFType active = active_EKF_type ( ) ;
if ( active ! = last_active_ekf_type ) {
last_active_ekf_type = active ;
2021-01-07 00:36:32 -04:00
const char * shortname = " ??? " ;
switch ( ( EKFType ) active ) {
case EKFType : : NONE :
shortname = " DCM " ;
break ;
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2021-10-29 21:38:13 -03:00
shortname = " SIM " ;
2021-01-07 00:36:32 -04:00
break ;
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
shortname = " External " ;
break ;
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
shortname = " EKF3 " ;
break ;
# endif
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
shortname = " EKF2 " ;
break ;
# endif
}
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " AHRS: %s active " , shortname ) ;
2020-12-30 18:42:44 -04:00
}
2015-09-22 22:40:25 -03:00
}
2021-08-18 07:20:46 -03:00
/*
* copy results from a backend over AP_AHRS canonical results .
* This updates member variables like roll and pitch , as well as
* updating derived values like sin_roll and sin_pitch .
*/
void AP_AHRS : : copy_estimates_from_backend_estimates ( const AP_AHRS_Backend : : Estimates & results )
2014-01-01 21:15:58 -04:00
{
2021-08-18 07:20:46 -03:00
roll = results . roll_rad ;
pitch = results . pitch_rad ;
yaw = results . yaw_rad ;
_dcm_matrix = results . dcm_matrix ;
_gyro_estimate = results . gyro_estimate ;
_gyro_drift = results . gyro_drift ;
2022-07-26 21:21:01 -03:00
_accel_ef = results . accel_ef ;
2021-12-06 05:54:12 -04:00
_accel_bias = results . accel_bias ;
2021-08-18 07:20:46 -03:00
2014-10-14 23:18:08 -03:00
update_cd_values ( ) ;
2021-08-18 07:20:46 -03:00
update_trig ( ) ;
}
2014-10-14 21:15:33 -03:00
2021-08-18 07:20:46 -03:00
void AP_AHRS : : update_DCM ( )
{
dcm . update ( ) ;
dcm . get_results ( dcm_estimates ) ;
// we always update the vehicle's canonical roll/pitch/yaw from
// DCM. In normal operation this will usually be over-written by
// an EKF or external AHRS. This is long-held behaviour, but this
// really shouldn't be doing this.
2014-01-01 22:47:40 -04:00
2021-08-18 07:20:46 -03:00
// if (active_EKF_type() == EKFType::NONE) {
copy_estimates_from_backend_estimates ( dcm_estimates ) ;
// }
2015-09-22 22:40:25 -03:00
}
2021-08-27 10:22:05 -03:00
void AP_AHRS : : update_notify_from_filter_status ( const nav_filter_status & status )
{
AP_Notify : : flags . gps_fusion = status . flags . using_gps ; // Drives AP_Notify flag for usable GPS.
AP_Notify : : flags . gps_glitching = status . flags . gps_glitching ;
AP_Notify : : flags . have_pos_abs = status . flags . horiz_pos_abs ;
}
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2021-07-20 10:04:20 -03:00
void AP_AHRS : : update_EKF2 ( void )
2015-09-22 22:40:25 -03:00
{
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
}
2020-11-05 19:30:50 -04:00
// if we're doing Replay logging then don't allow any data
// into the EKF yet. Don't allow it to block us for long.
if ( ! hal . util - > was_watchdog_reset ( ) ) {
if ( AP_HAL : : millis ( ) - start_time_ms < 5000 ) {
if ( ! AP : : logger ( ) . allow_start_ekf ( ) ) {
return ;
}
2016-05-03 03:34:05 -03:00
}
2014-01-01 22:05:28 -04:00
}
2020-11-05 19:30:50 -04:00
if ( AP_HAL : : millis ( ) - start_time_ms > startup_delay_ms ) {
_ekf2_started = EKF2 . InitialiseFilter ( ) ;
}
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 ( ) ;
2019-12-12 18:45:45 -04:00
if ( active_EKF_type ( ) = = EKFType : : TWO ) {
2014-01-01 22:05:28 -04:00
Vector3f eulers ;
2015-11-20 03:34:00 -04:00
EKF2 . getRotationBodyToNED ( _dcm_matrix ) ;
2022-04-07 20:47:04 -03:00
EKF2 . getEulerAngles ( 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
2018-03-10 05:35:03 -04:00
const AP_InertialSensor & _ins = AP : : ins ( ) ;
2022-07-26 21:21:01 -03:00
const int8_t primary_imu = EKF2 . getPrimaryCoreIMUIndex ( ) ;
const uint8_t primary_gyro = primary_imu > = 0 ? primary_imu : _ins . get_primary_gyro ( ) ;
const uint8_t primary_accel = primary_imu > = 0 ? primary_imu : _ins . get_primary_accel ( ) ;
2018-03-10 05:35:03 -04:00
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 ( ) ;
2022-04-07 20:47:04 -03:00
EKF2 . getGyroBias ( _gyro_drift ) ;
2017-02-08 02:35:33 -04:00
_gyro_drift = - _gyro_drift ;
2016-09-03 04:12:39 -03:00
2022-07-26 21:21:01 -03:00
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins . get_gyro ( primary_gyro ) + _gyro_drift ;
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)
2021-12-06 05:54:12 -04:00
float & abias = _accel_bias . z ;
2022-04-07 20:47:04 -03:00
EKF2 . getAccelZBias ( 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
2022-07-26 21:21:01 -03:00
Vector3f accel = _ins . get_accel ( primary_accel ) ;
accel . z - = abias ;
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * accel ;
2017-06-01 19:02:59 -03:00
nav_filter_status filt_state ;
2022-04-07 20:47:04 -03:00
EKF2 . getFilterStatus ( filt_state ) ;
2021-08-27 10:22:05 -03:00
update_notify_from_filter_status ( filt_state ) ;
2014-01-01 22:05:28 -04:00
}
}
2014-01-01 21:15:58 -04:00
}
2019-12-12 03:31:43 -04:00
# endif
2014-01-01 21:15:58 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2021-07-20 10:04:20 -03:00
void AP_AHRS : : update_EKF3 ( void )
2016-07-14 02:08:42 -03:00
{
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 ( ) ;
}
2020-11-05 19:30:50 -04:00
// if we're doing Replay logging then don't allow any data
// into the EKF yet. Don't allow it to block us for long.
if ( ! hal . util - > was_watchdog_reset ( ) ) {
if ( AP_HAL : : millis ( ) - start_time_ms < 5000 ) {
if ( ! AP : : logger ( ) . allow_start_ekf ( ) ) {
return ;
}
2016-07-14 02:08:42 -03:00
}
}
2020-11-05 19:30:50 -04:00
if ( AP_HAL : : millis ( ) - start_time_ms > startup_delay_ms ) {
_ekf3_started = EKF3 . InitialiseFilter ( ) ;
}
2016-07-14 02:08:42 -03:00
}
2016-12-19 00:38:44 -04:00
if ( _ekf3_started ) {
2016-07-14 02:08:42 -03:00
EKF3 . UpdateFilter ( ) ;
2019-12-12 18:45:45 -04:00
if ( active_EKF_type ( ) = = EKFType : : THREE ) {
2016-07-14 02:08:42 -03:00
Vector3f eulers ;
EKF3 . getRotationBodyToNED ( _dcm_matrix ) ;
2022-04-05 00:33:32 -03:00
EKF3 . getEulerAngles ( eulers ) ;
2016-07-14 02:08:42 -03:00
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 ( ) ;
2022-07-26 21:21:01 -03:00
const uint8_t primary_gyro = primary_imu > = 0 ? primary_imu : _ins . get_primary_gyro ( ) ;
const uint8_t primary_accel = primary_imu > = 0 ? primary_imu : _ins . get_primary_accel ( ) ;
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
2022-07-26 21:21:01 -03:00
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins . get_gyro ( primary_gyro ) + _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)
2021-12-06 05:54:12 -04:00
Vector3f & abias = _accel_bias ;
2016-06-28 04:36:12 -03:00
EKF3 . getAccelBias ( - 1 , abias ) ;
2016-07-14 02:08:42 -03:00
2022-07-26 21:21:01 -03:00
// use the primary IMU for accel earth frame
Vector3f accel = _ins . get_accel ( primary_accel ) ;
accel - = abias ;
_accel_ef = _dcm_matrix * accel ;
2017-06-01 19:02:59 -03:00
nav_filter_status filt_state ;
2022-04-05 00:33:32 -03:00
EKF3 . getFilterStatus ( filt_state ) ;
2021-08-27 10:22:05 -03:00
update_notify_from_filter_status ( filt_state ) ;
2016-07-14 02:08:42 -03:00
}
}
}
2019-12-12 03:31:43 -04:00
# endif
2016-07-14 02:08:42 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-20 10:04:20 -03:00
void AP_AHRS : : update_SITL ( void )
2015-11-20 04:34:30 -04:00
{
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
2021-07-30 07:41:13 -03:00
if ( active_EKF_type ( ) = = EKFType : : SIM ) {
2019-03-18 14:55:16 -03:00
2017-04-15 08:21:09 -03:00
fdm . quaternion . rotation_matrix ( _dcm_matrix ) ;
2019-02-03 14:22:57 -04:00
_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body ( ) ;
_dcm_matrix . to_euler ( & roll , & pitch , & yaw ) ;
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
2022-07-26 21:21:01 -03:00
const Vector3f & accel = _ins . get_accel ( ) ;
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * accel ;
2017-03-21 19:38:50 -03:00
}
2017-04-30 21:09:57 -03:00
2020-04-11 19:26:48 -03:00
# if HAL_NAVEKF3_AVAILABLE
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
2020-05-08 20:06:13 -03:00
EKF3 . writeBodyFrameOdom ( quality , delPos , delAng , delTime , timeStamp_ms , 0 , posOffset ) ;
2017-04-30 21:09:57 -03:00
}
2015-11-20 04:34:30 -04:00
}
2020-04-11 19:26:48 -03:00
# endif // HAL_NAVEKF3_AVAILABLE
2015-11-20 04:34:30 -04:00
}
# endif // CONFIG_HAL_BOARD
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
2021-07-20 10:04:20 -03:00
void AP_AHRS : : update_external ( void )
2020-12-29 00:02:43 -04:00
{
2021-01-01 05:21:37 -04:00
AP : : externalAHRS ( ) . update ( ) ;
2020-12-29 00:02:43 -04:00
if ( active_EKF_type ( ) = = EKFType : : EXTERNAL ) {
Quaternion quat ;
if ( ! AP : : externalAHRS ( ) . get_quaternion ( quat ) ) {
return ;
}
quat . rotation_matrix ( _dcm_matrix ) ;
_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body ( ) ;
_dcm_matrix . to_euler ( & roll , & pitch , & yaw ) ;
update_cd_values ( ) ;
update_trig ( ) ;
_gyro_drift . zero ( ) ;
_gyro_estimate = AP : : externalAHRS ( ) . get_gyro ( ) ;
2022-07-26 21:21:01 -03:00
Vector3f accel = AP : : externalAHRS ( ) . get_accel ( ) ;
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * accel ;
2020-12-29 00:02:43 -04:00
}
}
# endif // HAL_EXTERNAL_AHRS_ENABLED
2021-08-18 07:20:46 -03:00
void AP_AHRS : : reset ( )
2014-01-01 21:15:58 -04:00
{
2018-08-19 22:18:03 -03:00
// support locked access functions to AHRS data
WITH_SEMAPHORE ( _rsem ) ;
2021-07-24 20:08:28 -03:00
2021-08-18 07:20:46 -03:00
dcm . reset ( ) ;
2021-07-24 20:08:28 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2016-12-19 00:38:44 -04:00
if ( _ekf2_started ) {
_ekf2_started = EKF2 . InitialiseFilter ( ) ;
2014-01-01 22:05:28 -04:00
}
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2016-12-19 00:38:44 -04:00
if ( _ekf3_started ) {
_ekf3_started = EKF3 . InitialiseFilter ( ) ;
2016-07-14 02:08:42 -03:00
}
2019-12-12 03:31:43 -04:00
# endif
2014-01-01 21:15:58 -04:00
}
// dead-reckoning support
2022-01-20 19:42:40 -04:00
bool AP_AHRS : : get_location ( struct Location & loc ) const
2014-01-01 21:15:58 -04:00
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2022-01-20 19:42:40 -04:00
return dcm . get_location ( loc ) ;
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2018-09-13 17:39:52 -03:00
if ( EKF2 . getLLH ( loc ) ) {
2015-09-22 22:40:25 -03:00
return true ;
}
break ;
2019-12-12 03:31:43 -04:00
# endif
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2018-09-13 17:39:52 -03:00
if ( EKF3 . getLLH ( loc ) ) {
2016-07-14 02:08:42 -03:00
return true ;
}
break ;
2019-12-12 03:31:43 -04:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
2018-06-06 21:01:02 -03:00
if ( _sitl ) {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
2019-01-01 23:26:19 -04:00
loc = { } ;
2018-06-06 21:01:02 -03:00
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
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL : {
return AP : : externalAHRS ( ) . get_location ( loc ) ;
}
# endif
2014-01-01 22:05:28 -04:00
}
2020-04-24 06:36:46 -03:00
// fall back to position from DCM
if ( ! always_use_EKF ( ) ) {
2022-01-20 19:42:40 -04:00
return dcm . get_location ( loc ) ;
2020-04-24 06:36:46 -03:00
}
return false ;
2014-01-01 21:15:58 -04:00
}
2014-01-01 22:05:28 -04:00
// status reporting of estimated errors
2021-07-20 10:04:20 -03:00
float AP_AHRS : : get_error_rp ( void ) const
2014-01-01 21:15:58 -04:00
{
2021-08-18 07:20:46 -03:00
return dcm . get_error_rp ( ) ;
2014-01-01 21:15:58 -04:00
}
2021-07-20 10:04:20 -03:00
float AP_AHRS : : get_error_yaw ( void ) const
2014-01-01 21:15:58 -04:00
{
2021-08-18 07:20:46 -03:00
return dcm . get_error_yaw ( ) ;
2014-01-01 21:15:58 -04:00
}
// return a wind estimation vector, in m/s
2021-07-20 10:04:20 -03:00
Vector3f AP_AHRS : : 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 ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
wind = dcm . wind_estimate ( ) ;
2015-09-22 22:40:25 -03:00
break ;
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2022-07-29 22:45:47 -03:00
if ( _sitl ) {
const auto & fdm = _sitl - > state ;
wind = fdm . wind_ef ;
}
2015-11-20 04:34:30 -04:00
break ;
# endif
2016-04-03 22:08:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
EKF2 . getWind ( wind ) ;
2016-04-03 22:08:03 -03:00
break ;
2019-12-12 03:31:43 -04:00
# endif
2016-04-03 22:08:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
EKF3 . getWind ( wind ) ;
2016-06-28 04:36:12 -03:00
break ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
wind . zero ( ) ;
break ;
# endif
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
}
2022-01-26 02:36:13 -04:00
/*
return true if a real airspeed sensor is enabled
*/
bool AP_AHRS : : airspeed_sensor_enabled ( void ) const
{
if ( ! dcm . airspeed_sensor_enabled ( ) ) {
return false ;
}
nav_filter_status filter_status ;
if ( fly_forward & &
hal . util - > get_soft_armed ( ) & &
get_filter_status ( filter_status ) & &
filter_status . flags . rejecting_airspeed ) {
// special case for when backend is rejecting airspeed data in
// an armed fly_forward state. Then the airspeed data is
// highly suspect and will be rejected. We will use the
// synthentic airspeed instead
return false ;
}
return true ;
}
2014-01-01 21:15:58 -04:00
// return an airspeed estimate if available. return true
// if we have an estimate
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : airspeed_estimate ( float & airspeed_ret ) const
2020-08-27 03:06:38 -03:00
{
2020-06-06 20:24:22 -03:00
bool ret = false ;
2022-01-09 21:37:03 -04:00
# if AP_AIRSPEED_ENABLED
2020-06-06 20:24:22 -03:00
if ( airspeed_sensor_enabled ( ) ) {
2021-11-23 19:18:11 -04:00
uint8_t idx = get_active_airspeed_index ( ) ;
airspeed_ret = AP : : airspeed ( ) - > get_airspeed ( idx ) ;
2021-08-24 00:38:57 -03:00
if ( _wind_max > 0 & & AP : : gps ( ) . status ( ) > = AP_GPS : : GPS_OK_FIX_2D ) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
const float gnd_speed = AP : : gps ( ) . ground_speed ( ) ;
float true_airspeed = airspeed_ret * get_EAS2TAS ( ) ;
true_airspeed = constrain_float ( true_airspeed ,
gnd_speed - _wind_max ,
gnd_speed + _wind_max ) ;
airspeed_ret = true_airspeed / get_EAS2TAS ( ) ;
}
2020-06-06 20:24:22 -03:00
return true ;
}
2022-01-09 21:37:03 -04:00
# endif
2020-06-06 20:24:22 -03:00
2021-08-16 23:34:15 -03:00
if ( ! get_wind_estimation_enabled ( ) ) {
2020-06-06 20:24:22 -03:00
return false ;
}
// estimate it via nav velocity and wind estimates
// get wind estimates
Vector3f wind_vel ;
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . airspeed_estimate ( get_active_airspeed_index ( ) , airspeed_ret ) ;
2020-06-06 20:24:22 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2021-08-24 00:01:16 -03:00
if ( ! _sitl ) {
return false ;
2020-06-06 20:24:22 -03:00
}
2021-08-24 00:01:16 -03:00
airspeed_ret = _sitl - > state . airspeed ;
return true ;
2020-06-06 20:24:22 -03:00
# endif
2020-12-12 18:10:33 -04:00
# if HAL_NAVEKF2_AVAILABLE
2020-06-06 20:24:22 -03:00
case EKFType : : TWO :
2021-08-18 07:20:46 -03:00
return dcm . airspeed_estimate ( get_active_airspeed_index ( ) , airspeed_ret ) ;
2020-12-12 18:10:33 -04:00
# endif
2020-06-06 20:24:22 -03:00
2020-12-12 18:10:33 -04:00
# if HAL_NAVEKF3_AVAILABLE
2020-06-06 20:24:22 -03:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
ret = EKF3 . getWind ( wind_vel ) ;
2020-06-06 20:24:22 -03:00
break ;
2020-12-12 18:10:33 -04:00
# endif
2021-01-05 19:26:05 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
# endif
2020-06-06 20:24:22 -03:00
}
// estimate it via nav velocity and wind estimates
Vector3f nav_vel ;
float true_airspeed ;
2021-02-28 00:43:36 -04:00
if ( ret & & have_inertial_nav ( ) & & get_velocity_NED ( nav_vel ) ) {
Vector3f true_airspeed_vec = nav_vel - wind_vel ;
true_airspeed = true_airspeed_vec . length ( ) ;
float gnd_speed = nav_vel . length ( ) ;
if ( _wind_max > 0 ) {
float tas_lim_lower = MAX ( 0.0f , ( gnd_speed - _wind_max ) ) ;
float tas_lim_upper = MAX ( tas_lim_lower , ( gnd_speed + _wind_max ) ) ;
true_airspeed = constrain_float ( true_airspeed , tas_lim_lower , tas_lim_upper ) ;
2020-06-06 20:24:22 -03:00
} else {
2021-02-28 00:43:36 -04:00
true_airspeed = MAX ( 0.0f , true_airspeed ) ;
2020-06-06 20:24:22 -03:00
}
2021-02-28 00:43:36 -04:00
airspeed_ret = true_airspeed / get_EAS2TAS ( ) ;
} else {
// fallback to DCM if airspeed estimate if EKF has wind but no velocity estimate
2021-08-18 07:20:46 -03:00
ret = dcm . airspeed_estimate ( get_active_airspeed_index ( ) , airspeed_ret ) ;
2020-06-06 20:24:22 -03:00
}
return ret ;
2014-01-01 21:15:58 -04:00
}
2021-08-18 07:20:46 -03:00
bool AP_AHRS : : airspeed_estimate_true ( float & airspeed_ret ) const
{
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
return dcm . airspeed_estimate_true ( airspeed_ret ) ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-08-18 07:20:46 -03:00
case EKFType : : SIM :
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
break ;
}
if ( ! airspeed_estimate ( airspeed_ret ) ) {
return false ;
}
airspeed_ret * = get_EAS2TAS ( ) ;
return true ;
}
2020-11-20 17:40:19 -04:00
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : airspeed_vector_true ( Vector3f & vec ) const
2020-11-20 17:40:19 -04:00
{
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
break ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
return EKF2 . getAirSpdVec ( vec ) ;
2020-11-20 17:40:19 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
return EKF3 . getAirSpdVec ( vec ) ;
2020-11-20 17:40:19 -04:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-11-20 17:40:19 -04:00
break ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
break ;
# endif
2020-11-20 17:40:19 -04:00
}
return false ;
}
2021-08-18 07:20:46 -03:00
// return a synthetic airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool AP_AHRS : : synthetic_airspeed ( float & ret ) const
{
return dcm . synthetic_airspeed ( ret ) ;
}
2014-01-01 21:15:58 -04:00
// true if compass is being used
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : use_compass ( void )
2014-01-01 21:15:58 -04:00
{
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2015-09-22 22:40:25 -03:00
break ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-09-22 22:40:25 -03:00
return EKF2 . use_compass ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-07-14 02:08:42 -03:00
return EKF3 . use_compass ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return true ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
// fall through
break ;
# endif
2014-08-24 08:00:56 -03:00
}
2021-08-18 07:20:46 -03:00
return dcm . use_compass ( ) ;
2014-01-01 21:15:58 -04:00
}
2020-03-31 22:08:54 -03:00
// return the quaternion defining the rotation from NED to XYZ (body) axes
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_quaternion ( Quaternion & quat ) const
2020-03-31 22:08:54 -03:00
{
2021-07-23 02:42:45 -03:00
// backends always return in autopilot XYZ frame; rotate result
// according to trim
2020-03-31 22:08:54 -03:00
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
if ( ! dcm . get_quaternion ( quat ) ) {
2021-07-23 02:42:45 -03:00
return false ;
}
break ;
2020-03-31 22:08:54 -03:00
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
2021-07-23 02:42:45 -03:00
if ( ! _ekf2_started ) {
return false ;
}
2022-04-07 20:47:04 -03:00
EKF2 . getQuaternion ( quat ) ;
2021-07-23 02:42:45 -03:00
break ;
2020-03-31 22:08:54 -03:00
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
2021-07-23 02:42:45 -03:00
if ( ! _ekf3_started ) {
return false ;
}
2022-04-05 00:33:32 -03:00
EKF3 . getQuaternion ( quat ) ;
2021-07-23 02:42:45 -03:00
break ;
2020-03-31 22:08:54 -03:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
2021-07-23 02:42:45 -03:00
if ( ! _sitl ) {
2020-03-31 22:08:54 -03:00
return false ;
}
2021-07-23 02:42:45 -03:00
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
quat = fdm . quaternion ;
break ;
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
2021-07-23 02:42:45 -03:00
// we assume the external AHRS isn't trimmed with the autopilot!
2020-12-29 00:02:43 -04:00
return AP : : externalAHRS ( ) . get_quaternion ( quat ) ;
2020-03-31 22:08:54 -03:00
# endif
}
2021-07-23 02:42:45 -03:00
quat . rotate ( - _trim . get ( ) ) ;
return true ;
2020-03-31 22:08:54 -03:00
}
2014-01-01 22:47:40 -04:00
// return secondary attitude solution if available, as eulers in radians
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_secondary_attitude ( Vector3f & eulers ) const
2014-01-01 22:47:40 -04:00
{
2020-11-26 22:37:04 -04:00
EKFType secondary_ekf_type ;
if ( ! get_secondary_EKF_type ( secondary_ekf_type ) ) {
return false ;
}
switch ( secondary_ekf_type ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2020-11-26 22:37:04 -04:00
// DCM is secondary
2021-08-18 07:20:46 -03:00
eulers [ 0 ] = dcm_estimates . roll_rad ;
eulers [ 1 ] = dcm_estimates . pitch_rad ;
eulers [ 2 ] = dcm_estimates . yaw_rad ;
2020-11-26 22:37:04 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2020-11-26 22:37:04 -04:00
// EKF2 is secondary
2022-04-07 20:47:04 -03:00
EKF2 . getEulerAngles ( eulers ) ;
2020-11-26 22:37:04 -04:00
return _ekf2_started ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2020-11-26 22:37:04 -04:00
// EKF3 is secondary
2022-04-05 00:33:32 -03:00
EKF3 . getEulerAngles ( eulers ) ;
2020-11-26 22:37:04 -04:00
return _ekf3_started ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-11-26 22:37:04 -04:00
// SITL is secondary (should never happen)
return false ;
2020-12-29 00:02:43 -04:00
# endif
2020-11-26 22:37:04 -04:00
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
2020-11-26 22:37:04 -04:00
case EKFType : : EXTERNAL : {
// External is secondary
Quaternion quat ;
if ( ! AP : : externalAHRS ( ) . get_quaternion ( quat ) ) {
return false ;
}
quat . to_euler ( eulers . x , eulers . y , eulers . z ) ;
2016-12-22 18:47:46 -04:00
return true ;
2014-01-01 22:47:40 -04:00
}
2020-11-26 22:37:04 -04:00
# endif
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2014-01-01 22:47:40 -04:00
}
2017-04-15 08:21:09 -03:00
// return secondary attitude solution if available, as quaternion
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_secondary_quaternion ( Quaternion & quat ) const
2017-04-15 08:21:09 -03:00
{
2020-11-26 22:37:04 -04:00
EKFType secondary_ekf_type ;
if ( ! get_secondary_EKF_type ( secondary_ekf_type ) ) {
2019-12-12 03:31:43 -04:00
return false ;
2020-11-26 22:37:04 -04:00
}
switch ( secondary_ekf_type ) {
case EKFType : : NONE :
// DCM is secondary
2021-08-18 07:20:46 -03:00
if ( ! dcm . get_quaternion ( quat ) ) {
2021-07-23 02:42:45 -03:00
return false ;
}
break ;
2017-04-15 08:21:09 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2020-11-26 22:37:04 -04:00
// EKF2 is secondary
2021-07-23 02:42:45 -03:00
if ( ! _ekf2_started ) {
return false ;
}
2022-04-07 20:47:04 -03:00
EKF2 . getQuaternion ( quat ) ;
2021-07-23 02:42:45 -03:00
break ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-15 08:21:09 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2020-11-26 22:37:04 -04:00
// EKF3 is secondary
2021-07-23 02:42:45 -03:00
if ( ! _ekf3_started ) {
return false ;
}
2022-04-05 00:33:32 -03:00
EKF3 . getQuaternion ( quat ) ;
2021-07-23 02:42:45 -03:00
break ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-15 08:21:09 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-11-26 22:37:04 -04:00
// SITL is secondary (should never happen)
return false ;
2020-12-29 00:02:43 -04:00
# endif
2020-11-26 22:37:04 -04:00
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
2020-11-26 22:37:04 -04:00
// External is secondary
return AP : : externalAHRS ( ) . get_quaternion ( quat ) ;
2019-12-12 05:54:12 -04:00
# endif
2017-04-15 08:21:09 -03:00
}
2020-11-26 22:37:04 -04:00
2021-07-23 02:42:45 -03:00
quat . rotate ( - _trim . get ( ) ) ;
return true ;
2017-04-15 08:21:09 -03:00
}
2014-01-01 22:47:40 -04:00
// return secondary position solution if available
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_secondary_position ( struct Location & loc ) const
2014-01-01 22:47:40 -04:00
{
2020-11-26 22:37:04 -04:00
EKFType secondary_ekf_type ;
if ( ! get_secondary_EKF_type ( secondary_ekf_type ) ) {
2019-12-12 03:31:43 -04:00
return false ;
2020-11-26 22:37:04 -04:00
}
switch ( secondary_ekf_type ) {
case EKFType : : NONE :
// return DCM position
2022-01-20 19:42:40 -04:00
dcm . get_location ( loc ) ;
2020-11-26 22:37:04 -04:00
return true ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2020-11-26 22:37:04 -04:00
// EKF2 is secondary
EKF2 . getLLH ( loc ) ;
return _ekf2_started ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2020-11-26 22:37:04 -04:00
// EKF3 is secondary
EKF3 . getLLH ( loc ) ;
return _ekf3_started ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-11-26 22:37:04 -04:00
// SITL is secondary (should never happen)
return false ;
2020-12-29 00:02:43 -04:00
# endif
2020-11-26 22:37:04 -04:00
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
2020-11-26 22:37:04 -04:00
// External is secondary
return AP : : externalAHRS ( ) . get_location ( loc ) ;
2019-12-12 05:54:12 -04:00
# endif
2015-09-23 04:29:43 -03:00
}
2020-11-26 22:37:04 -04:00
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2014-01-01 22:47:40 -04:00
}
2014-01-01 23:25:41 -04:00
// EKF has a better ground speed vector estimate
2021-07-20 10:04:20 -03:00
Vector2f AP_AHRS : : groundspeed_vector ( void )
2014-01-01 23:25:41 -04:00
{
2015-09-22 22:40:25 -03:00
Vector3f vec ;
2015-09-23 04:53:44 -03:00
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
break ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
EKF2 . getVelNED ( vec ) ;
2015-09-22 22:40:25 -03:00
return Vector2f ( vec . x , vec . y ) ;
2019-12-12 03:31:43 -04:00
# endif
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
EKF3 . getVelNED ( vec ) ;
2016-07-14 02:08:42 -03:00
return Vector2f ( vec . x , vec . y ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-07-14 02:08:42 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
2018-06-06 21:01:02 -03:00
if ( _sitl ) {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
return Vector2f ( fdm . speedN , fdm . speedE ) ;
}
2019-12-12 18:45:45 -04:00
break ;
2015-11-20 04:34:30 -04:00
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL : {
return AP : : externalAHRS ( ) . get_groundspeed_vector ( ) ;
}
2015-11-20 04:34:30 -04:00
# endif
2014-01-01 23:25:41 -04:00
}
2021-08-18 07:20:46 -03:00
return dcm . groundspeed_vector ( ) ;
}
float AP_AHRS : : groundspeed ( void )
{
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
return dcm . groundspeed ( ) ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-08-18 07:20:46 -03:00
case EKFType : : SIM :
# endif
break ;
}
return groundspeed_vector ( ) . length ( ) ;
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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : set_origin ( const Location & loc )
2017-04-19 03:28:14 -03:00
{
2021-07-02 12:04:02 -03:00
WITH_SEMAPHORE ( _rsem ) ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2017-12-03 06:10:19 -04:00
const bool ret2 = EKF2 . setOriginLLH ( loc ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2017-12-03 06:10:19 -04:00
const bool ret3 = EKF3 . setOriginLLH ( loc ) ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-19 03:28:14 -03:00
// return success if active EKF's origin was set
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2019-12-12 05:54:12 -04:00
return false ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2017-04-19 03:28:14 -03:00
return ret2 ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-19 03:28:14 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2017-04-19 03:28:14 -03:00
return ret3 ;
2019-12-12 03:31:43 -04:00
# endif
2017-04-19 03:28:14 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-07-05 08:05:48 -03:00
// never allow origin set in SITL. The origin is set by the
// simulation backend
return false ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
// don't allow origin set with external AHRS
return false ;
2017-05-02 06:36:15 -03:00
# endif
2017-04-19 03:28:14 -03:00
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2017-04-19 03:28:14 -03:00
}
2014-01-03 20:15:34 -04:00
// return true if inertial navigation is active
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : have_inertial_nav ( void ) const
2014-01-03 20:15:34 -04:00
{
2019-12-12 18:45:45 -04:00
return active_EKF_type ( ) ! = EKFType : : 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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : 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 ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
break ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
EKF2 . getVelNED ( vec ) ;
2016-12-22 18:47:46 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
EKF3 . getVelNED ( vec ) ;
2016-12-22 18:47:46 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2016-07-14 02:08:42 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
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 ;
2020-12-29 00:02:43 -04:00
}
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return AP : : externalAHRS ( ) . get_velocity_NED ( vec ) ;
2015-11-20 04:34:30 -04:00
# endif
2014-01-03 20:15:34 -04:00
}
2021-08-18 07:20:46 -03:00
return dcm . get_velocity_NED ( vec ) ;
2014-01-03 20:15:34 -04:00
}
2016-01-04 19:57:17 -04:00
// returns the expected NED magnetic field
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_mag_field_NED ( Vector3f & vec ) const
2016-01-04 19:57:17 -04:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2016-12-22 18:47:46 -04:00
return false ;
2016-01-04 19:57:17 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
EKF2 . getMagNED ( vec ) ;
2016-12-22 18:47:46 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2016-01-04 19:57:17 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
EKF3 . getMagNED ( vec ) ;
2016-12-22 18:47:46 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2016-01-04 19:57:17 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2016-12-22 18:47:46 -04:00
return false ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
2016-01-04 19:57:17 -04:00
# endif
}
2019-12-12 18:45:45 -04:00
return false ;
2016-01-04 19:57:17 -04:00
}
// returns the estimated magnetic field offsets in body frame
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_mag_field_correction ( Vector3f & vec ) const
2016-01-04 19:57:17 -04:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2016-12-22 18:47:46 -04:00
return false ;
2016-01-04 19:57:17 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
EKF2 . getMagXYZ ( vec ) ;
2016-12-22 18:47:46 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2016-01-04 19:57:17 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
EKF3 . getMagXYZ ( vec ) ;
2016-12-22 18:47:46 -04:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2016-01-04 19:57:17 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2016-12-22 18:47:46 -04:00
return false ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
2016-12-22 18:47:46 -04:00
# endif
2016-01-04 19:57:17 -04:00
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
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.
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_vert_pos_rate ( float & velocity ) const
2015-10-12 06:50:01 -03:00
{
switch ( active_EKF_type ( ) ) {
2021-08-18 07:20:46 -03:00
case EKFType : : NONE :
return dcm . get_vert_pos_rate ( velocity ) ;
2015-10-12 06:50:01 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
velocity = EKF2 . getPosDownDerivative ( ) ;
2015-10-12 06:50:01 -03:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2015-10-12 06:50:01 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
velocity = EKF3 . getPosDownDerivative ( ) ;
2016-07-14 02:08:42 -03:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2018-06-06 21:01:02 -03:00
if ( _sitl ) {
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
velocity = fdm . speedD ;
return true ;
} else {
return false ;
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return AP : : externalAHRS ( ) . get_speed_down ( velocity ) ;
2015-11-20 04:34:30 -04:00
# endif
2015-10-12 06:50:01 -03:00
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2015-10-12 06:50:01 -03:00
}
// get latest height above ground level estimate in metres and a validity flag
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_hagl ( float & height ) const
2015-10-12 06:50:01 -03:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2015-10-12 06:50:01 -03:00
return false ;
2015-10-15 02:10:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-10-12 06:50:01 -03:00
return EKF2 . getHAGL ( height ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-06-28 04:36:12 -03:00
return EKF3 . getHAGL ( height ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
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 ;
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL : {
return false ;
}
2015-11-20 04:34:30 -04:00
# endif
2015-10-12 06:50:01 -03:00
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
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.
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : 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 ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . get_relative_position_NED_origin ( vec ) ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO : {
2016-07-09 08:36:09 -03:00
Vector2f posNE ;
float posD ;
2022-04-07 20:47:04 -03:00
if ( EKF2 . getPosNE ( posNE ) & & EKF2 . getPosD ( 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
}
2019-12-12 03:31:43 -04:00
# endif
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE : {
2016-06-28 04:36:12 -03:00
Vector2f posNE ;
float posD ;
2022-04-05 00:33:32 -03:00
if ( EKF3 . getPosNE ( posNE ) & & EKF3 . getPosD ( posD ) ) {
2016-12-19 22:42:54 -04:00
// 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
}
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
2018-06-06 21:01:02 -03:00
if ( ! _sitl ) {
return false ;
}
2020-07-05 08:05:48 -03:00
Location loc , orgn ;
2022-01-20 19:42:40 -04:00
if ( ! get_location ( loc ) | |
2020-07-05 08:05:48 -03:00
! get_origin ( orgn ) ) {
return false ;
}
const Vector2f diff2d = orgn . get_distance_NE ( loc ) ;
2015-11-20 04:34:30 -04:00
const struct SITL : : sitl_fdm & fdm = _sitl - > state ;
vec = Vector3f ( diff2d . x , diff2d . y ,
2020-07-05 08:05:48 -03:00
- ( fdm . altitude - orgn . alt * 0.01f ) ) ;
2015-11-20 04:34:30 -04:00
return true ;
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL : {
2021-01-02 22:54:39 -04:00
auto & extahrs = AP : : externalAHRS ( ) ;
2020-12-29 00:02:43 -04:00
Location loc , orgn ;
2021-01-02 22:54:39 -04:00
if ( extahrs . get_origin ( orgn ) & &
extahrs . get_location ( loc ) ) {
2020-12-29 00:02:43 -04:00
const Vector2f diff2d = orgn . get_distance_NE ( loc ) ;
vec = Vector3f ( diff2d . x , diff2d . y ,
- ( loc . alt - orgn . alt ) * 0.01 ) ;
return true ;
}
return false ;
}
2015-11-20 04:34:30 -04:00
# endif
2014-01-03 20:15:34 -04:00
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
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.
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_relative_position_NED_home ( Vector3f & vec ) const
2017-01-30 15:06:13 -04:00
{
Location originLLH ;
Vector3f originNED ;
if ( ! get_relative_position_NED_origin ( originNED ) | |
! get_origin ( originLLH ) ) {
return false ;
}
2019-04-08 10:28:51 -03:00
const Vector3f offset = originLLH . get_distance_NED ( _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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_relative_position_NE_origin ( Vector2f & posNE ) const
2016-07-09 08:36:09 -03:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . get_relative_position_NE_origin ( posNE ) ;
2016-07-09 08:36:09 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO : {
2022-04-07 20:47:04 -03:00
bool position_is_valid = EKF2 . getPosNE ( posNE ) ;
2016-07-09 08:36:09 -03:00
return position_is_valid ;
}
2019-12-12 03:31:43 -04:00
# endif
2016-07-09 08:36:09 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE : {
2022-04-05 00:33:32 -03:00
bool position_is_valid = EKF3 . getPosNE ( posNE ) ;
2016-06-28 04:36:12 -03:00
return position_is_valid ;
}
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
2020-07-05 08:05:48 -03:00
Location loc , orgn ;
2022-01-20 19:42:40 -04:00
if ( ! get_location ( loc ) | |
2020-07-05 08:05:48 -03:00
! get_origin ( orgn ) ) {
return false ;
}
posNE = orgn . get_distance_NE ( loc ) ;
2016-07-09 08:36:09 -03:00
return true ;
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL : {
Location loc , orgn ;
2022-01-20 19:42:40 -04:00
if ( ! get_location ( loc ) | |
2020-12-29 00:02:43 -04:00
! get_origin ( orgn ) ) {
return false ;
}
posNE = orgn . get_distance_NE ( loc ) ;
return true ;
}
2016-07-09 08:36:09 -03:00
# endif
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2016-07-09 08:36:09 -03:00
}
2017-01-30 15:06:13 -04:00
// return a relative ground position to the home in meters
// North/East order.
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_relative_position_NE_home ( Vector2f & posNE ) const
2017-01-30 15:06:13 -04:00
{
Location originLLH ;
Vector2f originNE ;
if ( ! get_relative_position_NE_origin ( originNE ) | |
! get_origin ( originLLH ) ) {
return false ;
}
2019-04-08 10:16:19 -03:00
const Vector2f offset = originLLH . get_distance_NE ( _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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_relative_position_D_origin ( float & posD ) const
2016-07-09 08:36:09 -03:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . get_relative_position_D_origin ( posD ) ;
2016-07-09 08:36:09 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO : {
2022-04-07 20:47:04 -03:00
bool position_is_valid = EKF2 . getPosD ( posD ) ;
2016-07-09 08:36:09 -03:00
return position_is_valid ;
}
2019-12-12 03:31:43 -04:00
# endif
2016-07-09 08:36:09 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE : {
2022-04-05 00:33:32 -03:00
bool position_is_valid = EKF3 . getPosD ( posD ) ;
2016-06-28 04:36:12 -03:00
return position_is_valid ;
}
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
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 ;
2020-07-05 08:05:48 -03:00
Location orgn ;
if ( ! get_origin ( orgn ) ) {
return false ;
}
posD = - ( fdm . altitude - orgn . alt * 0.01f ) ;
2016-07-09 08:36:09 -03:00
return true ;
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL : {
Location orgn , loc ;
if ( ! get_origin ( orgn ) | |
2022-01-20 19:42:40 -04:00
! get_location ( loc ) ) {
2020-12-29 00:02:43 -04:00
return false ;
}
posD = - ( loc . alt - orgn . alt ) * 0.01 ;
return true ;
}
2016-07-09 08:36:09 -03:00
# endif
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2016-07-09 08:36:09 -03:00
}
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
2021-07-20 10:04:20 -03:00
void AP_AHRS : : get_relative_position_D_home ( float & posD ) const
2017-01-30 15:06:13 -04:00
{
Location originLLH ;
float originD ;
if ( ! get_relative_position_D_origin ( originD ) | |
! get_origin ( originLLH ) ) {
2021-08-24 05:05:05 -03:00
const auto & gps = AP : : gps ( ) ;
if ( _gps_use = = GPSUse : : EnableWithHeight & &
gps . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
posD = ( get_home ( ) . alt - gps . location ( ) . alt ) * 0.01 ;
} else {
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
*/
2021-07-20 10:04:20 -03:00
AP_AHRS : : EKFType AP_AHRS : : ekf_type ( void ) const
2014-02-14 18:24:12 -04:00
{
2019-12-12 18:45:45 -04:00
EKFType type = ( EKFType ) _ekf_type . get ( ) ;
switch ( type ) {
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2019-12-12 03:31:43 -04:00
return type ;
2016-12-20 19:52:12 -04:00
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return type ;
# endif
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2019-12-12 03:31:43 -04:00
return type ;
# endif
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
return type ;
2019-12-12 03:31:43 -04:00
# endif
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
if ( always_use_EKF ( ) ) {
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
return EKFType : : TWO ;
2019-12-12 03:31:43 -04:00
# elif HAL_NAVEKF3_AVAILABLE
return EKFType : : THREE ;
# endif
2019-12-12 18:45:45 -04:00
}
return EKFType : : NONE ;
2015-11-20 04:34:30 -04:00
}
2019-12-12 18:45:45 -04:00
// we can get to here if the user has mis-set AHRS_EKF_TYPE - any
2019-12-12 03:31:43 -04:00
// value above 3 will get to here. TWO is returned here for no
// better reason than "tradition".
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
return EKFType : : TWO ;
2019-12-12 03:31:43 -04:00
# elif HAL_NAVEKF3_AVAILABLE
return EKFType : : THREE ;
# else
return EKFType : : NONE ;
# endif
2015-09-22 22:40:25 -03:00
}
2021-07-20 10:04:20 -03:00
AP_AHRS : : EKFType AP_AHRS : : active_EKF_type ( void ) const
2015-09-22 22:40:25 -03:00
{
2019-12-12 18:45:45 -04:00
EKFType ret = EKFType : : NONE ;
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
return EKFType : : NONE ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO : {
2015-09-22 22:40:25 -03:00
// do we have an EKF2 yet?
2016-12-19 00:38:44 -04:00
if ( ! _ekf2_started ) {
2019-12-12 18:45:45 -04:00
return EKFType : : NONE ;
2015-09-22 22:40:25 -03:00
}
2015-10-13 15:08:49 -03:00
if ( always_use_EKF ( ) ) {
2016-05-10 03:05:50 -03:00
uint16_t ekf2_faults ;
2022-04-07 20:47:04 -03:00
EKF2 . getFilterFaults ( ekf2_faults ) ;
2015-10-13 15:08:49 -03:00
if ( ekf2_faults = = 0 ) {
2019-12-12 18:45:45 -04:00
ret = EKFType : : TWO ;
2015-10-13 15:08:49 -03:00
}
} else if ( EKF2 . healthy ( ) ) {
2019-12-12 18:45:45 -04:00
ret = EKFType : : TWO ;
2015-09-22 22:40:25 -03:00
}
break ;
}
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE : {
2016-07-14 02:08:42 -03:00
// do we have an EKF3 yet?
2016-12-19 00:38:44 -04:00
if ( ! _ekf3_started ) {
2019-12-12 18:45:45 -04:00
return EKFType : : NONE ;
2016-07-14 02:08:42 -03:00
}
if ( always_use_EKF ( ) ) {
uint16_t ekf3_faults ;
2022-04-05 00:33:32 -03:00
EKF3 . getFilterFaults ( ekf3_faults ) ;
2016-07-14 02:08:42 -03:00
if ( ekf3_faults = = 0 ) {
2019-12-12 18:45:45 -04:00
ret = EKFType : : THREE ;
2016-07-14 02:08:42 -03:00
}
} else if ( EKF3 . healthy ( ) ) {
2019-12-12 18:45:45 -04:00
ret = EKFType : : THREE ;
2016-07-14 02:08:42 -03:00
}
break ;
}
2019-12-12 03:31:43 -04:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
ret = EKFType : : SIM ;
2015-11-20 04:34:30 -04:00
break ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
ret = EKFType : : EXTERNAL ;
break ;
2015-11-20 04:34:30 -04:00
# endif
2015-09-22 22:40:25 -03:00
}
2016-03-04 17:52:40 -04:00
/*
2021-08-14 00:03:42 -03:00
fixed wing and rover will fall back to DCM if the EKF doesn ' t
have GPS . This is the safest option as 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
*/
2019-12-12 18:45:45 -04:00
if ( ret ! = EKFType : : NONE & &
2021-08-11 20:25:34 -03:00
( _vehicle_class = = VehicleClass : : FIXED_WING | |
2021-08-14 00:03:42 -03:00
_vehicle_class = = VehicleClass : : GROUND ) ) {
2020-11-24 01:55:24 -04:00
bool should_use_gps = true ;
2015-05-19 02:15:44 -03:00
nav_filter_status filt_state ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
if ( ret = = EKFType : : TWO ) {
2022-04-07 20:47:04 -03:00
EKF2 . getFilterStatus ( filt_state ) ;
2020-11-24 01:55:24 -04:00
should_use_gps = EKF2 . configuredToUseGPSForPosXY ( ) ;
2019-12-12 03:31:43 -04:00
}
# endif
# if HAL_NAVEKF3_AVAILABLE
if ( ret = = EKFType : : THREE ) {
2022-04-05 00:33:32 -03:00
EKF3 . getFilterStatus ( filt_state ) ;
2020-11-24 01:55:24 -04:00
should_use_gps = EKF3 . configuredToUseGPSForPosXY ( ) ;
2019-12-12 03:31:43 -04:00
}
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
if ( ret = = EKFType : : SIM ) {
2015-11-20 04:34:30 -04:00
get_filter_status ( filt_state ) ;
2015-09-22 22:40:25 -03:00
}
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
if ( ret = = EKFType : : EXTERNAL ) {
get_filter_status ( filt_state ) ;
should_use_gps = true ;
}
2019-12-12 03:31:43 -04:00
# endif
2020-08-21 02:08:51 -03:00
if ( hal . util - > get_soft_armed ( ) & &
( ! filt_state . flags . using_gps | |
! filt_state . flags . horiz_pos_abs ) & &
2020-11-24 01:55:24 -04:00
should_use_gps & &
2020-08-21 02:08:51 -03:00
AP : : gps ( ) . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
// if the EKF is not fusing GPS or doesn't have a 2D fix
// 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
2019-12-12 18:45:45 -04:00
return EKFType : : NONE ;
2015-05-19 02:15:44 -03:00
}
if ( hal . util - > get_soft_armed ( ) & & filt_state . flags . const_pos_mode ) {
2019-12-12 18:45:45 -04:00
return EKFType : : 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 ) {
2019-12-12 18:45:45 -04:00
return EKFType : : NONE ;
2016-05-26 21:18:53 -03:00
}
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 ) ) {
2021-07-25 21:32:10 -03:00
if ( ( ! AP : : 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 ;
}
}
2019-12-12 18:45:45 -04:00
return EKFType : : 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
}
2020-11-26 22:37:04 -04:00
// get secondary EKF type. returns false if no secondary (i.e. only using DCM)
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_secondary_EKF_type ( EKFType & secondary_ekf_type ) const
2020-11-26 22:37:04 -04:00
{
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
// EKF2, EKF3 or External is secondary
# if HAL_NAVEKF3_AVAILABLE
if ( ( EKFType ) _ekf_type . get ( ) = = EKFType : : THREE ) {
secondary_ekf_type = EKFType : : THREE ;
return true ;
}
# endif
# if HAL_NAVEKF2_AVAILABLE
if ( ( EKFType ) _ekf_type . get ( ) = = EKFType : : TWO ) {
secondary_ekf_type = EKFType : : TWO ;
return true ;
}
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
if ( ( EKFType ) _ekf_type . get ( ) = = EKFType : : EXTERNAL ) {
secondary_ekf_type = EKFType : : EXTERNAL ;
return true ;
}
# endif
return false ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-11-26 22:37:04 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
// DCM is secondary
secondary_ekf_type = EKFType : : NONE ;
return true ;
}
// since there is no default case above, this is unreachable
return false ;
}
2014-05-15 04:09:18 -03:00
/*
check if the AHRS subsystem is healthy
*/
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : 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 ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . healthy ( ) ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO : {
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 ;
}
2021-08-11 20:25:34 -03:00
if ( ( _vehicle_class = = VehicleClass : : FIXED_WING | |
_vehicle_class = = VehicleClass : : GROUND ) & &
2019-12-12 18:45:45 -04:00
active_EKF_type ( ) ! = EKFType : : TWO ) {
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 ;
}
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE : {
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 ;
}
2021-08-11 20:25:34 -03:00
if ( ( _vehicle_class = = VehicleClass : : FIXED_WING | |
_vehicle_class = = VehicleClass : : GROUND ) & &
2019-12-12 18:45:45 -04:00
active_EKF_type ( ) ! = EKFType : : THREE ) {
2016-07-14 02:08:42 -03:00
// on fixed wing we want to be using EKF to be considered
// healthy if EKF is enabled
return false ;
}
return true ;
}
2019-12-12 03:31:43 -04:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return true ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return AP : : externalAHRS ( ) . healthy ( ) ;
2015-11-20 04:34:30 -04:00
# endif
2015-09-22 22:40:25 -03:00
}
2021-08-18 07:20:46 -03:00
return dcm . healthy ( ) ;
2014-05-15 04:09:18 -03:00
}
2020-08-11 02:02:55 -03:00
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
2021-01-20 23:42:19 -04:00
// requires_position should be true if horizontal position configuration should be checked
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : pre_arm_check ( bool requires_position , char * failure_msg , uint8_t failure_msg_len ) const
2019-03-23 15:21:47 -03:00
{
2020-12-10 07:14:12 -04:00
bool ret = true ;
if ( ! healthy ( ) ) {
// this rather generic failure might be overwritten by
// something more specific in the "backend"
hal . util - > snprintf ( failure_msg , failure_msg_len , " Not healthy " ) ;
ret = false ;
}
2022-07-26 21:18:06 -03:00
if ( ! attitudes_consistent ( failure_msg , failure_msg_len ) ) {
return false ;
}
2022-08-05 00:14:11 -03:00
// ensure we're using the configured backend, but bypass in compass-less cases:
if ( ekf_type ( ) ! = active_EKF_type ( ) & & AP : : compass ( ) . use_for_yaw ( ) ) {
2022-07-26 21:18:06 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " AHRS: not using configured AHRS type " ) ;
return false ;
}
2019-03-23 15:21:47 -03:00
switch ( ekf_type ( ) ) {
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2021-08-18 07:20:46 -03:00
return ret ;
2019-03-23 15:21:47 -03:00
# endif
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2022-01-13 21:00:24 -04:00
return dcm . pre_arm_check ( requires_position , failure_msg , failure_msg_len ) & & ret ;
2019-03-23 15:21:47 -03:00
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
2022-01-13 21:00:24 -04:00
return AP : : externalAHRS ( ) . pre_arm_check ( failure_msg , failure_msg_len ) & & ret ;
2020-12-29 00:02:43 -04:00
# endif
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2020-08-11 02:02:55 -03:00
if ( ! _ekf2_started ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " EKF2 not started " ) ;
return false ;
2019-03-23 15:21:47 -03:00
}
2020-12-10 07:14:12 -04:00
return EKF2 . pre_arm_check ( failure_msg , failure_msg_len ) & & ret ;
2019-12-12 03:31:43 -04:00
# endif
2019-03-23 15:21:47 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2020-08-11 02:02:55 -03:00
if ( ! _ekf3_started ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " EKF3 not started " ) ;
return false ;
2019-03-23 15:21:47 -03:00
}
2021-01-20 23:42:19 -04:00
return EKF3 . pre_arm_check ( requires_position , failure_msg , failure_msg_len ) & & ret ;
2019-12-12 03:31:43 -04:00
# endif
2019-03-23 15:21:47 -03:00
}
2020-08-11 02:02:55 -03:00
// if we get here then ekf type is invalid
hal . util - > snprintf ( failure_msg , failure_msg_len , " invalid EKF type " ) ;
return false ;
2019-03-23 15:21:47 -03:00
}
2014-10-02 01:43:46 -03:00
// true if the AHRS has completed initialisation
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : initialised ( void ) const
2014-09-29 05:37:14 -03:00
{
2015-09-22 22:40:25 -03:00
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2015-09-22 22:40:25 -03:00
return true ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
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 ) ) ;
2019-12-12 03:31:43 -04:00
# endif
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-07-14 02:08:42 -03:00
// 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 ) ) ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return true ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return AP : : externalAHRS ( ) . initialised ( ) ;
2015-11-20 04:34:30 -04:00
# endif
2015-09-23 04:29:43 -03:00
}
2019-12-12 18:45:45 -04:00
return false ;
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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_filter_status ( nav_filter_status & status ) const
2015-10-12 06:50:01 -03:00
{
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . get_filter_status ( status ) ;
2015-10-15 02:10:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -03:00
EKF2 . getFilterStatus ( status ) ;
2015-10-15 02:10:03 -03:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2015-10-15 02:10:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
EKF3 . getFilterStatus ( status ) ;
2016-07-14 02:08:42 -03:00
return true ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
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 ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
AP : : externalAHRS ( ) . get_filter_status ( status ) ;
return true ;
2015-11-20 04:34:30 -04:00
# endif
2015-10-12 06:50:01 -03:00
}
2015-10-15 02:10:03 -03:00
2019-12-12 18:45:45 -04:00
return false ;
2015-10-12 06:50:01 -03:00
}
2014-08-06 21:31:24 -03:00
// write optical flow data to EKF
2021-07-20 10:04:20 -03:00
void AP_AHRS : : writeOptFlowMeas ( const uint8_t rawFlowQuality , const Vector2f & rawFlowRates , const Vector2f & rawGyroRates , const uint32_t msecFlowMeas , const Vector3f & posOffset )
2014-08-06 21:31:24 -03:00
{
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2016-10-11 18:16:10 -03:00
EKF2 . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas , posOffset ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2016-07-14 02:08:42 -03:00
EKF3 . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas , posOffset ) ;
2019-12-12 03:31:43 -04:00
# endif
2014-08-06 21:31:24 -03:00
}
2014-11-12 14:49:15 -04:00
2022-01-19 23:17:53 -04:00
// retrieve latest corrected optical flow samples (used for calibration)
bool AP_AHRS : : getOptFlowSample ( uint32_t & timeStamp_ms , Vector2f & flowRate , Vector2f & bodyRate , Vector2f & losPred ) const
{
# if HAL_NAVEKF3_AVAILABLE
return EKF3 . getOptFlowSample ( timeStamp_ms , flowRate , bodyRate , losPred ) ;
# endif
return false ;
}
2017-03-21 18:13:54 -03:00
// write body frame odometry measurements to the EKF
2021-07-20 10:04:20 -03:00
void AP_AHRS : : writeBodyFrameOdom ( float quality , const Vector3f & delPos , const Vector3f & delAng , float delTime , uint32_t timeStamp_ms , uint16_t delay_ms , const Vector3f & posOffset )
2017-03-21 18:13:54 -03:00
{
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2020-05-08 20:06:13 -03:00
EKF3 . writeBodyFrameOdom ( quality , delPos , delAng , delTime , timeStamp_ms , delay_ms , posOffset ) ;
2019-12-12 03:31:43 -04:00
# endif
2017-03-21 18:13:54 -03:00
}
2018-03-07 20:44:53 -04:00
// Write position and quaternion data from an external navigation system
2021-07-20 10:04:20 -03:00
void AP_AHRS : : writeExtNavData ( const Vector3f & pos , const Quaternion & quat , float posErr , float angErr , uint32_t timeStamp_ms , uint16_t delay_ms , uint32_t resetTime_ms )
2018-03-07 20:44:53 -04:00
{
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2020-04-13 02:03:01 -03:00
EKF2 . writeExtNavData ( pos , quat , posErr , angErr , timeStamp_ms , delay_ms , resetTime_ms ) ;
2019-12-12 03:31:43 -04:00
# endif
2020-04-14 21:56:35 -03:00
# if HAL_NAVEKF3_AVAILABLE
2020-04-13 02:03:01 -03:00
EKF3 . writeExtNavData ( pos , quat , posErr , angErr , timeStamp_ms , delay_ms , resetTime_ms ) ;
2020-04-14 21:56:35 -03:00
# endif
2018-03-07 20:44:53 -04:00
}
2020-06-06 20:24:22 -03:00
// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
2021-07-20 10:04:20 -03:00
void AP_AHRS : : writeDefaultAirSpeed ( float airspeed , float uncertainty )
2020-03-10 03:49:14 -03:00
{
# if HAL_NAVEKF2_AVAILABLE
EKF2 . writeDefaultAirSpeed ( airspeed ) ;
# endif
# if HAL_NAVEKF3_AVAILABLE
2020-06-06 20:24:22 -03:00
EKF3 . writeDefaultAirSpeed ( airspeed , uncertainty ) ;
2020-03-10 03:49:14 -03:00
# endif
}
2018-03-07 20:44:53 -04:00
2020-05-12 03:06:24 -03:00
// Write velocity data from an external navigation system
2021-07-20 10:04:20 -03:00
void AP_AHRS : : writeExtNavVelData ( const Vector3f & vel , float err , uint32_t timeStamp_ms , uint16_t delay_ms )
2020-05-12 03:06:24 -03:00
{
# if HAL_NAVEKF2_AVAILABLE
EKF2 . writeExtNavVelData ( vel , err , timeStamp_ms , delay_ms ) ;
# endif
2020-05-18 02:22:40 -03:00
# if HAL_NAVEKF3_AVAILABLE
EKF3 . writeExtNavVelData ( vel , err , timeStamp_ms , delay_ms ) ;
# endif
2020-05-12 03:06:24 -03:00
}
2021-08-14 00:03:42 -03:00
// get speed limit and XY navigation gain scale factor
void AP_AHRS : : getControlLimits ( float & ekfGndSpdLimit , float & ekfNavVelGainScaler ) const
2014-11-15 19:36:33 -04:00
{
2021-08-14 00:03:42 -03:00
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-14 00:03:42 -03:00
// lower gains in VTOL controllers when flying on DCM
ekfGndSpdLimit = 50.0 ;
ekfNavVelGainScaler = 0.5 ;
2019-12-12 18:45:45 -04:00
break ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-09-22 22:40:25 -03:00
EKF2 . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
break ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-06-28 04:36:12 -03:00
EKF3 . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2015-11-20 04:34:30 -04:00
break ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2016-12-29 03:43:32 -04:00
// same as EKF2 for no optical flow
ekfGndSpdLimit = 400.0f ;
ekfNavVelGainScaler = 1.0f ;
break ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
2021-08-14 00:03:42 -03:00
// no limit on gains, large vel limit
ekfGndSpdLimit = 400 ;
ekfNavVelGainScaler = 1 ;
2020-12-29 00:02:43 -04:00
break ;
2016-12-29 03:43:32 -04:00
# endif
2015-09-22 22:40:25 -03:00
}
2014-11-15 19:36:33 -04:00
}
2021-08-14 00:03:42 -03:00
/*
get gain factor for Z controllers
*/
float AP_AHRS : : getControlScaleZ ( void ) const
{
if ( active_EKF_type ( ) = = EKFType : : NONE ) {
// when flying on DCM lower gains by 4x to cope with the high
// lag
return 0.25 ;
}
return 1 ;
}
2015-03-16 18:29:11 -03:00
// get compass offset estimates
// true if offsets are valid
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : 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 ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
return false ;
2015-09-22 22:40:25 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2016-03-29 17:06:56 -03:00
return EKF2 . getMagOffsets ( mag_idx , magOffsets ) ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-06-28 04:36:12 -03:00
return EKF3 . getMagOffsets ( mag_idx , magOffsets ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
magOffsets . zero ( ) ;
return true ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
2015-11-20 04:34:30 -04:00
# endif
2015-09-22 22:40:25 -03:00
}
2019-12-12 18:45:45 -04:00
// since there is no default case above, this is unreachable
return false ;
2015-03-16 18:29:11 -03:00
}
2016-08-11 16:58:02 -03:00
// Retrieves the NED delta velocity corrected
2021-07-20 10:04:20 -03:00
void AP_AHRS : : getCorrectedDeltaVelocityNED ( Vector3f & ret , float & dt ) const
2016-08-11 16:58:02 -03:00
{
2019-12-12 03:31:43 -04:00
int8_t imu_idx = - 1 ;
Vector3f accel_bias ;
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
break ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
imu_idx = EKF2 . getPrimaryCoreIMUIndex ( ) ;
2022-04-07 20:47:04 -03:00
EKF2 . getAccelZBias ( accel_bias . z ) ;
2019-12-12 03:31:43 -04:00
break ;
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
imu_idx = EKF3 . getPrimaryCoreIMUIndex ( ) ;
EKF3 . getAccelBias ( - 1 , accel_bias ) ;
break ;
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2019-12-12 03:31:43 -04:00
break ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
break ;
2019-12-12 03:31:43 -04:00
# endif
}
if ( imu_idx = = - 1 ) {
2021-08-18 07:20:46 -03:00
dcm . getCorrectedDeltaVelocityNED ( ret , dt ) ;
2019-12-12 03:31:43 -04:00
return ;
2016-08-11 16:58:02 -03:00
}
2019-12-12 03:31:43 -04:00
ret . zero ( ) ;
2021-02-11 17:34:36 -04:00
AP : : ins ( ) . get_delta_velocity ( ( uint8_t ) imu_idx , ret , dt ) ;
2019-12-12 03:31:43 -04:00
ret - = accel_bias * dt ;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body ( ) * ret ;
ret . z + = GRAVITY_MSS * dt ;
2016-08-11 16:58:02 -03:00
}
2019-02-19 23:52:55 -04:00
// check all cores providing consistent attitudes for prearm checks
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : attitudes_consistent ( char * failure_msg , const uint8_t failure_msg_len ) const
2019-02-19 23:52:55 -04:00
{
// get primary attitude source's attitude as quaternion
Quaternion primary_quat ;
2019-04-03 12:40:37 -03:00
get_quat_body_to_ned ( primary_quat ) ;
2019-03-06 02:50:11 -04:00
// only check yaw if compasses are being used
2021-07-25 21:32:10 -03:00
const bool check_yaw = AP : : compass ( ) . use_for_yaw ( ) ;
2020-05-10 21:33:00 -03:00
uint8_t total_ekf_cores = 0 ;
2019-03-06 02:50:11 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-02-19 23:52:55 -04:00
// check primary vs ekf2
2021-10-18 22:12:09 -03:00
if ( ekf_type ( ) = = EKFType : : TWO | | active_EKF_type ( ) = = EKFType : : TWO ) {
for ( uint8_t i = 0 ; i < EKF2 . activeCores ( ) ; i + + ) {
Quaternion ekf2_quat ;
EKF2 . getQuaternionBodyToNED ( i , ekf2_quat ) ;
// check roll and pitch difference
const float rp_diff_rad = primary_quat . roll_pitch_difference ( ekf2_quat ) ;
if ( rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " EKF2 Roll/Pitch inconsistent by %d deg " , ( int ) degrees ( rp_diff_rad ) ) ;
return false ;
}
2021-06-02 02:39:56 -03:00
2021-10-18 22:12:09 -03:00
// check yaw difference
Vector3f angle_diff ;
primary_quat . angular_difference ( ekf2_quat ) . to_axis_angle ( angle_diff ) ;
const float yaw_diff = fabsf ( angle_diff . z ) ;
if ( check_yaw & & ( yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " EKF2 Yaw inconsistent by %d deg " , ( int ) degrees ( yaw_diff ) ) ;
return false ;
}
2019-02-19 23:52:55 -04:00
}
2021-10-18 22:12:09 -03:00
total_ekf_cores = EKF2 . activeCores ( ) ;
2019-02-19 23:52:55 -04:00
}
2019-12-12 03:31:43 -04:00
# endif
2019-02-19 23:52:55 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-02-19 23:52:55 -04:00
// check primary vs ekf3
2021-10-18 22:12:09 -03:00
if ( ekf_type ( ) = = EKFType : : THREE | | active_EKF_type ( ) = = EKFType : : THREE ) {
for ( uint8_t i = 0 ; i < EKF3 . activeCores ( ) ; i + + ) {
Quaternion ekf3_quat ;
EKF3 . getQuaternionBodyToNED ( i , ekf3_quat ) ;
// check roll and pitch difference
const float rp_diff_rad = primary_quat . roll_pitch_difference ( ekf3_quat ) ;
if ( rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " EKF3 Roll/Pitch inconsistent by %d deg " , ( int ) degrees ( rp_diff_rad ) ) ;
return false ;
}
2021-06-02 02:39:56 -03:00
2021-10-18 22:12:09 -03:00
// check yaw difference
Vector3f angle_diff ;
primary_quat . angular_difference ( ekf3_quat ) . to_axis_angle ( angle_diff ) ;
const float yaw_diff = fabsf ( angle_diff . z ) ;
if ( check_yaw & & ( yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " EKF3 Yaw inconsistent by %d deg " , ( int ) degrees ( yaw_diff ) ) ;
return false ;
}
2019-02-19 23:52:55 -04:00
}
2021-10-18 22:12:09 -03:00
total_ekf_cores + = EKF3 . activeCores ( ) ;
2019-02-19 23:52:55 -04:00
}
2019-12-12 03:31:43 -04:00
# endif
2019-02-19 23:52:55 -04:00
// check primary vs dcm
2020-05-10 21:33:00 -03:00
if ( ! always_use_EKF ( ) | | ( total_ekf_cores = = 1 ) ) {
Quaternion dcm_quat ;
dcm_quat . from_rotation_matrix ( get_DCM_rotation_body_to_ned ( ) ) ;
2021-06-02 02:39:56 -03:00
// check roll and pitch difference
const float rp_diff_rad = primary_quat . roll_pitch_difference ( dcm_quat ) ;
if ( rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " DCM Roll/Pitch inconsistent by %d deg " , ( int ) degrees ( rp_diff_rad ) ) ;
2020-05-10 21:33:00 -03:00
return false ;
}
2020-04-04 20:54:44 -03:00
2020-05-10 21:33:00 -03:00
// Check vs DCM yaw if this vehicle could use DCM in flight
// and if not using an external yaw source (DCM does not support external yaw sources)
2021-08-18 04:34:37 -03:00
bool using_noncompass_for_yaw = false ;
2020-04-11 19:26:48 -03:00
# if HAL_NAVEKF3_AVAILABLE
2021-08-18 04:34:37 -03:00
using_noncompass_for_yaw = ( ekf_type ( ) = = EKFType : : THREE ) & & EKF3 . using_noncompass_for_yaw ( ) ;
2020-04-11 19:26:48 -03:00
# endif
2021-08-18 04:34:37 -03:00
if ( ! always_use_EKF ( ) & & ! using_noncompass_for_yaw ) {
2021-06-02 02:39:56 -03:00
Vector3f angle_diff ;
primary_quat . angular_difference ( dcm_quat ) . to_axis_angle ( angle_diff ) ;
const float yaw_diff = fabsf ( angle_diff . z ) ;
if ( check_yaw & & ( yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " DCM Yaw inconsistent by %d deg " , ( int ) degrees ( yaw_diff ) ) ;
2020-05-10 21:33:00 -03:00
return false ;
}
2020-04-04 20:54:44 -03:00
}
2019-02-19 23:52:55 -04:00
}
return true ;
}
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
2021-07-20 10:04:20 -03:00
uint32_t AP_AHRS : : getLastYawResetAngle ( float & yawAng )
2015-09-23 04:46:51 -03:00
{
2021-08-14 00:03:42 -03:00
switch ( active_EKF_type ( ) ) {
2016-06-28 04:36:12 -03:00
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . getLastYawResetAngle ( yawAng ) ;
2019-12-12 18:45:45 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-09-23 04:46:51 -03:00
return EKF2 . getLastYawResetAngle ( yawAng ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-07-14 02:08:42 -03:00
return EKF3 . getLastYawResetAngle ( yawAng ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return 0 ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return 0 ;
2015-11-20 04:34:30 -04:00
# 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
2021-07-20 10:04:20 -03:00
uint32_t AP_AHRS : : getLastPosNorthEastReset ( Vector2f & pos )
2015-10-29 02:18:04 -03:00
{
2021-08-14 00:03:42 -03:00
switch ( active_EKF_type ( ) ) {
2016-06-28 04:36:12 -03:00
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
return 0 ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-10-29 02:18:04 -03:00
return EKF2 . getLastPosNorthEastReset ( pos ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-07-14 02:08:42 -03:00
return EKF3 . getLastPosNorthEastReset ( pos ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return 0 ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return 0 ;
2015-11-20 04:34:30 -04:00
# 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
2021-07-20 10:04:20 -03:00
uint32_t AP_AHRS : : getLastVelNorthEastReset ( Vector2f & vel ) const
2015-10-29 02:18:04 -03:00
{
2021-08-14 00:03:42 -03:00
switch ( active_EKF_type ( ) ) {
2016-06-28 04:36:12 -03:00
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
return 0 ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-10-29 02:18:04 -03:00
return EKF2 . getLastVelNorthEastReset ( vel ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-07-14 02:08:42 -03:00
return EKF3 . getLastVelNorthEastReset ( vel ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return 0 ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return 0 ;
2015-11-20 04:34:30 -04:00
# 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
2021-07-20 10:04:20 -03:00
uint32_t AP_AHRS : : getLastPosDownReset ( float & posDelta )
2016-11-22 01:28:14 -04:00
{
2021-08-14 00:03:42 -03:00
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
return 0 ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2016-11-22 01:28:14 -04:00
return EKF2 . getLastPosDownReset ( posDelta ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-06-28 04:36:12 -03:00
return EKF3 . getLastPosDownReset ( posDelta ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2016-11-22 01:28:14 -04:00
return 0 ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return 0 ;
2016-11-22 01:28:14 -04:00
# 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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : resetHeightDatum ( void )
2015-09-23 04:46:51 -03:00
{
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
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
EKF3 . resetHeightDatum ( ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
EKF2 . resetHeightDatum ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2019-12-12 18:45:45 -04:00
return false ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2016-07-14 02:08:42 -03:00
EKF3 . resetHeightDatum ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2015-09-23 04:46:51 -03:00
return EKF2 . resetHeightDatum ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2016-07-14 02:08:42 -03:00
EKF2 . resetHeightDatum ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-07-14 02:08:42 -03:00
return EKF3 . resetHeightDatum ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return false ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
2015-11-20 04:34:30 -04:00
# 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
2022-07-08 01:15:35 -03:00
void AP_AHRS : : send_ekf_status_report ( GCS_MAVLINK & link ) const
2015-09-28 21:58:54 -03:00
{
2017-09-01 04:43:03 -03:00
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2016-06-28 04:36:12 -03:00
// send zero status report
2022-07-08 01:15:35 -03:00
dcm . send_ekf_status_report ( link ) ;
2016-06-28 04:36:12 -03:00
break ;
2015-09-28 21:58:54 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2019-07-02 17:38:52 -03:00
{
// send status report with everything looking good
const uint16_t flags =
EKF_ATTITUDE | /* Set if EKF's attitude estimate is good. | */
EKF_VELOCITY_HORIZ | /* Set if EKF's horizontal velocity estimate is good. | */
EKF_VELOCITY_VERT | /* Set if EKF's vertical velocity estimate is good. | */
EKF_POS_HORIZ_REL | /* Set if EKF's horizontal position (relative) estimate is good. | */
EKF_POS_HORIZ_ABS | /* Set if EKF's horizontal position (absolute) estimate is good. | */
EKF_POS_VERT_ABS | /* Set if EKF's vertical position (absolute) estimate is good. | */
EKF_POS_VERT_AGL | /* Set if EKF's vertical position (above ground) estimate is good. | */
//EKF_CONST_POS_MODE | /* EKF is in constant position mode and does not know it's absolute or relative position. | */
EKF_PRED_POS_HORIZ_REL | /* Set if EKF's predicted horizontal position (relative) estimate is good. | */
EKF_PRED_POS_HORIZ_ABS ; /* Set if EKF's predicted horizontal position (absolute) estimate is good. | */
2022-07-08 01:15:35 -03:00
mavlink_msg_ekf_status_report_send ( link . get_chan ( ) , flags , 0 , 0 , 0 , 0 , 0 , 0 ) ;
2019-07-02 17:38:52 -03:00
}
2016-06-19 09:02:25 -03:00
break ;
# endif
2019-12-12 03:31:43 -04:00
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
2021-01-01 02:24:13 -04:00
case EKFType : : EXTERNAL : {
2022-07-08 01:15:35 -03:00
AP : : externalAHRS ( ) . send_status_report ( link ) ;
2020-12-29 00:02:43 -04:00
break ;
2021-01-01 02:24:13 -04:00
}
2020-12-29 00:02:43 -04:00
# endif
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-07-08 01:15:35 -03:00
return EKF2 . send_status_report ( link ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-07-08 01:15:35 -03:00
return EKF3 . send_status_report ( link ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
}
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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_origin ( Location & ret ) const
2015-10-12 06:50:01 -03:00
{
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2021-08-18 07:20:46 -03:00
return dcm . get_origin ( ret ) ;
2015-10-15 02:10:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2022-04-07 20:47:04 -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 ;
2019-12-12 03:31:43 -04:00
# endif
2015-10-15 02:10:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
if ( ! EKF3 . getOriginLLH ( ret ) ) {
2016-07-14 02:08:42 -03:00
return false ;
}
return true ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM : {
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 ;
2020-12-29 00:02:43 -04:00
}
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return AP : : externalAHRS ( ) . get_origin ( ret ) ;
2015-11-20 04:34:30 -04:00
# endif
2015-10-12 06:50:01 -03:00
}
2019-12-12 18:45:45 -04:00
return false ;
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
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_hgt_ctrl_limit ( float & limit ) const
2015-10-12 06:50:01 -03:00
{
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2015-10-12 06:50:01 -03:00
// We are not using an EKF so no limiting applies
return false ;
2015-10-15 02:10:03 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2015-10-12 06:50:01 -03:00
return EKF2 . getHeightControlLimit ( limit ) ;
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-06-28 04:36:12 -03:00
return EKF3 . getHeightControlLimit ( limit ) ;
2019-12-12 03:31:43 -04:00
# endif
2016-06-28 04:36:12 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
return false ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
2015-11-20 04:34:30 -04:00
# endif
2015-10-12 06:50:01 -03:00
}
2019-12-12 18:45:45 -04:00
return false ;
2015-10-12 06:50:01 -03:00
}
2020-06-22 00:15:07 -03:00
// Set to true if the terrain underneath is stable enough to be used as a height reference
// this is not related to terrain following
2021-07-20 10:04:20 -03:00
void AP_AHRS : : set_terrain_hgt_stable ( bool stable )
2020-06-22 00:15:07 -03:00
{
2020-11-07 17:31:39 -04:00
// avoid repeatedly setting variable in NavEKF objects to prevent
// spurious event logging
2020-11-05 19:34:17 -04:00
switch ( terrainHgtStableState ) {
case TriState : : UNKNOWN :
break ;
case TriState : : True :
if ( stable ) {
return ;
}
break ;
case TriState : : False :
if ( ! stable ) {
return ;
}
break ;
}
terrainHgtStableState = ( TriState ) stable ;
2020-06-22 00:15:07 -03:00
# if HAL_NAVEKF2_AVAILABLE
EKF2 . setTerrainHgtStable ( stable ) ;
# endif
# if HAL_NAVEKF3_AVAILABLE
EKF3 . setTerrainHgtStable ( stable ) ;
# endif
}
2019-10-04 22:04:00 -03:00
// return the innovations for the primariy EKF
// boolean false is returned if innovations are not available
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_innovations ( Vector3f & velInnov , Vector3f & posInnov , Vector3f & magInnov , float & tasInnov , float & yawInnov ) const
2019-10-04 22:04:00 -03:00
{
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2019-10-04 22:04:00 -03:00
// We are not using an EKF so no data
return false ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2019-10-04 22:04:00 -03:00
// use EKF to get innovations
2022-04-07 20:47:04 -03:00
return EKF2 . getInnovations ( velInnov , posInnov , magInnov , tasInnov , yawInnov ) ;
2019-12-12 03:31:43 -04:00
# endif
2019-10-04 22:04:00 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2019-10-04 22:04:00 -03:00
// use EKF to get innovations
2022-04-05 00:33:32 -03:00
return EKF3 . getInnovations ( velInnov , posInnov , magInnov , tasInnov , yawInnov ) ;
2019-12-12 03:31:43 -04:00
# endif
2019-10-04 22:04:00 -03:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2019-10-04 22:04:00 -03:00
velInnov . zero ( ) ;
posInnov . zero ( ) ;
magInnov . zero ( ) ;
tasInnov = 0.0f ;
yawInnov = 0.0f ;
return true ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
# endif
2019-10-04 22:04:00 -03:00
}
2019-12-12 18:45:45 -04:00
return false ;
2019-10-04 22:04:00 -03:00
}
2021-07-16 03:23:47 -03:00
// returns true when the state estimates are significantly degraded by vibration
bool AP_AHRS : : is_vibration_affected ( ) const
{
switch ( ekf_type ( ) ) {
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
2022-04-05 00:33:32 -03:00
return EKF3 . isVibrationAffected ( ) ;
2021-07-16 03:23:47 -03:00
# endif
case EKFType : : NONE :
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2021-07-16 03:23:47 -03:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
return false ;
}
return false ;
}
2015-10-17 02:49:50 -03:00
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
2022-02-03 21:27:48 -04:00
// indicates prefect consistency between the measurement and the EKF solution and a value of 1 is the maximum
2015-10-17 02:49:50 -03:00
// inconsistency that will be accpeted by the filter
// boolean false is returned if variances are not available
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_variances ( float & velVar , float & posVar , float & hgtVar , Vector3f & magVar , float & tasVar ) const
2015-10-17 02:49:50 -03:00
{
switch ( ekf_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2015-10-17 02:49:50 -03:00
// We are not using an EKF so no data
return false ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2020-10-20 01:23:42 -03:00
case EKFType : : TWO : {
2015-10-17 02:49:50 -03:00
// use EKF to get variance
2020-10-20 01:23:42 -03:00
Vector2f offset ;
2022-04-07 20:47:04 -03:00
return EKF2 . getVariances ( velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
2020-10-20 01:23:42 -03:00
}
2019-12-12 03:31:43 -04:00
# endif
2015-10-17 02:49:50 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2020-10-20 01:23:42 -03:00
case EKFType : : THREE : {
2016-07-14 02:08:42 -03:00
// use EKF to get variance
2020-10-20 01:23:42 -03:00
Vector2f offset ;
2022-04-05 00:33:32 -03:00
return EKF3 . getVariances ( velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
2020-10-20 01:23:42 -03:00
}
2019-12-12 03:31:43 -04:00
# endif
2015-11-20 04:34:30 -04:00
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2015-11-20 04:34:30 -04:00
velVar = 0 ;
posVar = 0 ;
hgtVar = 0 ;
magVar . zero ( ) ;
tasVar = 0 ;
return true ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
# endif
2015-10-17 02:49:50 -03:00
}
2019-12-12 18:45:45 -04:00
return false ;
2015-10-17 02:49:50 -03:00
}
2020-10-13 02:21:07 -03:00
// get a source's velocity innovations. source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY)
// returns true on success and results are placed in innovations and variances arguments
2021-07-20 10:04:20 -03:00
bool AP_AHRS : : get_vel_innovations_and_variances_for_source ( uint8_t source , Vector3f & innovations , Vector3f & variances ) const
2020-10-13 02:21:07 -03:00
{
switch ( ekf_type ( ) ) {
case EKFType : : NONE :
// We are not using an EKF so no data
return false ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
// EKF2 does not support source level variances
return false ;
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
// use EKF to get variance
2022-04-05 00:33:32 -03:00
return EKF3 . getVelInnovationsAndVariancesForSource ( ( AP_NavEKF_Source : : SourceXY ) source , innovations , variances ) ;
2020-10-13 02:21:07 -03:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-10-13 02:21:07 -03:00
// SITL does not support source level variances
return false ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
return false ;
# endif
2020-10-13 02:21:07 -03:00
}
return false ;
}
2020-08-18 12:08:35 -03:00
//get the index of the active airspeed sensor, wrt the primary core
2021-07-20 10:04:20 -03:00
uint8_t AP_AHRS : : get_active_airspeed_index ( ) const
2020-08-18 12:08:35 -03:00
{
2022-01-09 21:37:03 -04:00
# if AP_AIRSPEED_ENABLED
2021-11-23 19:18:11 -04:00
const auto * airspeed = AP : : airspeed ( ) ;
if ( airspeed = = nullptr ) {
return 0 ;
}
2020-08-18 12:08:35 -03:00
// we only have affinity for EKF3 as of now
# if HAL_NAVEKF3_AVAILABLE
if ( active_EKF_type ( ) = = EKFType : : THREE ) {
2022-04-05 00:33:32 -03:00
uint8_t ret = EKF3 . getActiveAirspeed ( ) ;
2021-11-23 19:18:11 -04:00
if ( ret ! = 255 & & airspeed - > healthy ( ret ) ) {
return ret ;
}
2020-08-18 12:08:35 -03:00
}
# endif
2022-01-09 21:37:03 -04:00
2020-08-18 12:08:35 -03:00
// for the rest, let the primary airspeed sensor be used
2021-11-23 19:18:11 -04:00
return airspeed - > get_primary ( ) ;
2022-01-09 21:37:03 -04:00
# else
return 0 ;
# endif // AP_AIRSPEED_ENABLED
2020-08-18 12:08:35 -03:00
}
2017-03-02 07:33:13 -04:00
// get the index of the current primary IMU
2021-07-20 10:04:20 -03:00
uint8_t AP_AHRS : : get_primary_IMU_index ( ) const
2016-09-03 04:12:39 -03:00
{
int8_t imu = - 1 ;
2022-07-26 04:50:35 -03:00
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2022-07-26 04:50:35 -03:00
imu = AP : : ins ( ) . get_primary_gyro ( ) ;
2019-12-12 18:45:45 -04:00
break ;
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2016-09-03 04:12:39 -03:00
// let EKF2 choose primary IMU
imu = EKF2 . getPrimaryCoreIMUIndex ( ) ;
break ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2016-06-28 04:36:12 -03:00
// let EKF2 choose primary IMU
imu = EKF3 . getPrimaryCoreIMUIndex ( ) ;
break ;
2019-12-12 03:31:43 -04:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2016-09-03 04:12:39 -03:00
break ;
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
break ;
2019-12-12 18:45:45 -04:00
# endif
2016-09-03 04:12:39 -03:00
}
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 ;
}
2020-04-21 01:39:45 -03:00
// return the index of the primary core or -1 if no primary core selected
2021-07-20 10:04:20 -03:00
int8_t AP_AHRS : : get_primary_core_index ( ) const
2020-04-21 01:39:45 -03:00
{
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-04-21 01:39:45 -03:00
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
// we have only one core
2020-04-21 01:39:45 -03:00
return 0 ;
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
return EKF2 . getPrimaryCoreIndex ( ) ;
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
return EKF3 . getPrimaryCoreIndex ( ) ;
# endif
}
// we should never get here
2020-04-29 21:40:45 -03:00
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
2020-04-21 01:39:45 -03:00
return - 1 ;
}
2016-09-03 04:54:36 -03:00
// get the index of the current primary accelerometer sensor
2021-07-20 10:04:20 -03:00
uint8_t AP_AHRS : : get_primary_accel_index ( void ) const
2016-09-03 04:54:36 -03:00
{
2019-12-12 18:45:45 -04:00
if ( ekf_type ( ) ! = EKFType : : NONE ) {
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
2021-07-20 10:04:20 -03:00
uint8_t AP_AHRS : : get_primary_gyro_index ( void ) const
2016-09-03 04:54:36 -03:00
{
2019-12-12 18:45:45 -04:00
if ( ekf_type ( ) ! = EKFType : : NONE ) {
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
}
2019-06-07 20:33:45 -03:00
// see if EKF lane switching is possible to avoid EKF failsafe
2021-07-20 10:04:20 -03:00
void AP_AHRS : : check_lane_switch ( void )
2019-06-07 20:33:45 -03:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2019-06-07 20:33:45 -03:00
break ;
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2019-06-07 20:33:45 -03:00
break ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
break ;
# endif
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2019-06-07 20:33:45 -03:00
EKF2 . checkLaneSwitch ( ) ;
break ;
2019-12-12 03:31:43 -04:00
# endif
2019-06-07 20:33:45 -03:00
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2019-06-07 20:33:45 -03:00
EKF3 . checkLaneSwitch ( ) ;
break ;
2019-12-12 03:31:43 -04:00
# endif
2019-06-07 20:33:45 -03:00
}
}
2020-03-11 00:46:52 -03:00
// request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
2021-07-20 10:04:20 -03:00
void AP_AHRS : : request_yaw_reset ( void )
2020-03-11 00:46:52 -03:00
{
switch ( active_EKF_type ( ) ) {
case EKFType : : NONE :
break ;
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-03-11 00:46:52 -03:00
break ;
# endif
2020-12-29 00:02:43 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
break ;
# endif
2020-03-11 00:46:52 -03:00
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
EKF2 . requestYawReset ( ) ;
break ;
# endif
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
EKF3 . requestYawReset ( ) ;
break ;
# endif
}
}
2020-06-17 03:35:07 -03:00
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
2021-07-20 10:04:20 -03:00
void AP_AHRS : : set_posvelyaw_source_set ( uint8_t source_set_idx )
2020-06-17 03:35:07 -03:00
{
# if HAL_NAVEKF3_AVAILABLE
EKF3 . setPosVelYawSourceSet ( source_set_idx ) ;
# endif
}
2022-08-06 12:11:34 -03:00
//returns active source set used, 0=primary, 1=secondary, 2=tertiary
uint8_t AP_AHRS : : get_posvelyaw_source_set ( ) const
{
# if HAL_NAVEKF3_AVAILABLE
return EKF3 . get_active_source_set ( ) ;
# else
return 0 ;
# endif
}
2021-07-20 10:04:20 -03:00
void AP_AHRS : : Log_Write ( )
2019-07-02 20:54:23 -03:00
{
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2021-07-19 21:55:09 -03:00
EKF2 . Log_Write ( ) ;
2019-12-12 03:31:43 -04:00
# endif
# if HAL_NAVEKF3_AVAILABLE
2021-07-19 21:55:09 -03:00
EKF3 . Log_Write ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2022-01-24 02:23:44 -04:00
Write_AHRS2 ( ) ;
Write_POS ( ) ;
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2022-01-24 02:23:44 -04:00
AP : : sitl ( ) - > Log_Write_SIMSTATE ( ) ;
# endif
2019-07-02 20:54:23 -03:00
}
2018-04-06 09:17:04 -03:00
2021-08-18 04:34:37 -03:00
// check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
bool AP_AHRS : : using_noncompass_for_yaw ( void ) const
2019-07-24 06:48:07 -03:00
{
switch ( active_EKF_type ( ) ) {
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF2_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : TWO :
2019-07-24 06:48:07 -03:00
return EKF2 . isExtNavUsedForYaw ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2019-12-12 18:45:45 -04:00
case EKFType : : NONE :
2019-12-12 03:31:43 -04:00
# if HAL_NAVEKF3_AVAILABLE
2019-12-12 18:45:45 -04:00
case EKFType : : THREE :
2021-08-18 04:34:37 -03:00
return EKF3 . using_noncompass_for_yaw ( ) ;
2019-12-12 03:31:43 -04:00
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-07-30 07:41:13 -03:00
case EKFType : : SIM :
2020-12-29 00:02:43 -04:00
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
2019-12-12 18:45:45 -04:00
# endif
2019-07-24 06:48:07 -03:00
return false ;
}
2019-12-12 05:54:12 -04:00
// since there is no default case above, this is unreachable
return false ;
2019-07-24 06:48:07 -03:00
}
2021-08-18 04:45:01 -03:00
// check if external nav is providing yaw
bool AP_AHRS : : using_extnav_for_yaw ( void ) const
{
switch ( active_EKF_type ( ) ) {
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
return EKF2 . isExtNavUsedForYaw ( ) ;
# endif
case EKFType : : NONE :
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
return EKF3 . using_extnav_for_yaw ( ) ;
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-08-18 04:45:01 -03:00
case EKFType : : SIM :
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
return false ;
}
// since there is no default case above, this is unreachable
return false ;
}
2020-04-21 20:57:07 -03:00
// set and save the alt noise parameter value
2021-07-20 10:04:20 -03:00
void AP_AHRS : : set_alt_measurement_noise ( float noise )
2020-04-21 20:57:07 -03:00
{
# if HAL_NAVEKF2_AVAILABLE
EKF2 . set_baro_alt_noise ( noise ) ;
# endif
# if HAL_NAVEKF3_AVAILABLE
EKF3 . set_baro_alt_noise ( noise ) ;
# endif
}
2021-07-20 10:04:20 -03:00
2021-10-14 05:48:04 -03:00
// check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
const EKFGSF_yaw * AP_AHRS : : get_yaw_estimator ( void ) const
{
switch ( active_EKF_type ( ) ) {
# if HAL_NAVEKF2_AVAILABLE
case EKFType : : TWO :
return EKF2 . get_yawEstimator ( ) ;
# endif
case EKFType : : NONE :
# if HAL_NAVEKF3_AVAILABLE
case EKFType : : THREE :
return EKF3 . get_yawEstimator ( ) ;
# endif
2021-10-29 21:38:13 -03:00
# if AP_AHRS_SIM_ENABLED
2021-10-14 05:48:04 -03:00
case EKFType : : SIM :
# endif
# if HAL_EXTERNAL_AHRS_ENABLED
case EKFType : : EXTERNAL :
# endif
return nullptr ;
}
// since there is no default case above, this is unreachable
return nullptr ;
}
2021-07-20 10:04:20 -03:00
// singleton instance
AP_AHRS * AP_AHRS : : _singleton ;
namespace AP {
AP_AHRS & ahrs ( )
{
return * AP_AHRS : : get_singleton ( ) ;
}
}