2015-10-06 18:20:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF2_core.h"
# include <AP_AHRS/AP_AHRS.h>
2019-06-13 23:44:54 -03:00
# include <AP_GPS/AP_GPS.h>
2019-11-26 02:43:24 -04:00
# include <AP_RangeFinder/AP_RangeFinder.h>
2015-10-06 18:20:43 -03:00
extern const AP_HAL : : HAL & hal ;
// Check basic filter health metrics and return a consolidated health status
bool NavEKF2_core : : healthy ( void ) const
{
2016-05-10 03:00:36 -03:00
uint16_t faultInt ;
2015-10-06 18:20:43 -03:00
getFilterFaults ( faultInt ) ;
if ( faultInt > 0 ) {
return false ;
}
if ( velTestRatio > 1 & & posTestRatio > 1 & & hgtTestRatio > 1 ) {
// all three metrics being above 1 means the filter is
// extremely unhealthy.
return false ;
}
// Give the filter a second to settle before use
if ( ( imuSampleTime_ms - ekfStartTime_ms ) < 1000 ) {
return false ;
}
2015-11-05 21:12:08 -04:00
// position and height innovations must be within limits when on-ground and in a static mode of operation
2015-10-06 18:20:43 -03:00
float horizErrSq = sq ( innovVelPos [ 3 ] ) + sq ( innovVelPos [ 4 ] ) ;
2015-11-05 21:12:08 -04:00
if ( onGround & & ( PV_AidingMode = = AID_NONE ) & & ( ( horizErrSq > 1.0f ) | | ( fabsf ( hgtInnovFiltState ) > 1.0f ) ) ) {
2015-10-06 18:20:43 -03:00
return false ;
}
// all OK
return true ;
}
2016-11-21 18:21:01 -04:00
// Return a consolidated error score where higher numbers represent larger errors
2015-11-05 21:46:04 -04:00
// Intended to be used by the front-end to determine which is the primary EKF
2016-11-21 18:21:01 -04:00
float NavEKF2_core : : errorScore ( ) const
2015-11-05 21:46:04 -04:00
{
float score = 0.0f ;
2016-11-21 18:21:01 -04:00
if ( tiltAlignComplete & & yawAlignComplete ) {
2016-11-23 17:55:55 -04:00
// Check GPS fusion performance
score = MAX ( score , 0.5f * ( velTestRatio + posTestRatio ) ) ;
// Check altimeter fusion performance
2016-11-21 18:21:01 -04:00
score = MAX ( score , hgtTestRatio ) ;
2016-11-23 17:55:55 -04:00
// Check attitude corrections
2016-11-21 18:21:01 -04:00
const float tiltErrThreshold = 0.05f ;
score = MAX ( score , tiltErrFilt / tiltErrThreshold ) ;
2015-11-05 21:46:04 -04:00
}
return score ;
}
2015-10-06 18:20:43 -03:00
// return data for debugging optical flow fusion
void NavEKF2_core : : getFlowDebug ( float & varFlow , float & gndOffset , float & flowInnovX , float & flowInnovY , float & auxInnov , float & HAGL , float & rngInnov , float & range , float & gndOffsetErr ) const
{
2015-11-27 13:11:58 -04:00
varFlow = MAX ( flowTestRatio [ 0 ] , flowTestRatio [ 1 ] ) ;
2015-10-06 18:20:43 -03:00
gndOffset = terrainState ;
flowInnovX = innovOptFlow [ 0 ] ;
flowInnovY = innovOptFlow [ 1 ] ;
2019-02-12 20:55:41 -04:00
auxInnov = norm ( auxFlowObsInnov . x , auxFlowObsInnov . y ) ;
2015-10-06 18:20:43 -03:00
HAGL = terrainState - stateStruct . position . z ;
rngInnov = innovRng ;
2015-11-12 04:07:52 -04:00
range = rangeDataDelayed . rng ;
2015-10-06 18:20:43 -03:00
gndOffsetErr = sqrtf ( Popt ) ; // note Popt is constrained to be non-negative in EstimateTerrainOffset()
}
2016-10-25 17:54:29 -03:00
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
2016-11-30 02:54:55 -04:00
bool NavEKF2_core : : getRangeBeaconDebug ( uint8_t & ID , float & rng , float & innov , float & innovVar , float & testRatio , Vector3f & beaconPosNED , float & offsetHigh , float & offsetLow )
2016-10-25 17:54:29 -03:00
{
2016-11-28 05:00:05 -04:00
// if the states have not been initialised or we have not received any beacon updates then return zeros
if ( ! statesInitialised | | N_beacons = = 0 ) {
2016-11-30 02:54:55 -04:00
return false ;
2016-11-28 05:00:05 -04:00
}
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
2016-10-25 17:54:29 -03:00
if ( rngBcnFuseDataReportIndex > = N_beacons ) {
rngBcnFuseDataReportIndex = 0 ;
}
2016-11-28 05:00:05 -04:00
// Output the fusion status data for the specified beacon
2016-10-25 17:54:29 -03:00
ID = rngBcnFuseDataReportIndex ; // beacon identifier
rng = rngBcnFusionReport [ rngBcnFuseDataReportIndex ] . rng ; // measured range to beacon (m)
innov = rngBcnFusionReport [ rngBcnFuseDataReportIndex ] . innov ; // range innovation (m)
innovVar = rngBcnFusionReport [ rngBcnFuseDataReportIndex ] . innovVar ; // innovation variance (m^2)
testRatio = rngBcnFusionReport [ rngBcnFuseDataReportIndex ] . testRatio ; // innovation consistency test ratio
beaconPosNED = rngBcnFusionReport [ rngBcnFuseDataReportIndex ] . beaconPosNED ; // beacon NED position
2016-11-30 02:54:55 -04:00
offsetHigh = bcnPosOffsetMax ; // beacon system vertical pos offset upper estimate
2016-10-25 17:54:29 -03:00
offsetLow = bcnPosOffsetMin ; // beacon system vertical pos offset lower estimate
rngBcnFuseDataReportIndex + + ;
2016-11-30 02:54:55 -04:00
return true ;
2016-10-25 17:54:29 -03:00
}
2015-10-06 18:20:43 -03:00
// provides the height limit to be observed by the control loops
// returns false if no height limiting is required
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
bool NavEKF2_core : : getHeightControlLimit ( float & height ) const
{
2018-03-07 00:42:31 -04:00
// only ask for limiting if we are doing optical flow only navigation
if ( frontend - > _fusionModeGPS = = 3 & & ( PV_AidingMode = = AID_RELATIVE ) & & flowDataValid ) {
2015-10-06 18:20:43 -03:00
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
2019-11-26 02:43:24 -04:00
const RangeFinder * _rng = AP : : rangefinder ( ) ;
if ( _rng = = nullptr ) {
// we really, really shouldn't be here.
return false ;
}
height = MAX ( float ( _rng - > max_distance_cm_orient ( ROTATION_PITCH_270 ) ) * 0.007f - 1.0f , 1.0f ) ;
2015-11-12 04:07:52 -04:00
// If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
if ( frontend - > _altSource ! = 1 ) {
height - = terrainState ;
}
2015-10-06 18:20:43 -03:00
return true ;
} else {
return false ;
}
}
2015-10-07 15:27:09 -03:00
// return the Euler roll, pitch and yaw angle in radians
void NavEKF2_core : : getEulerAngles ( Vector3f & euler ) const
{
outputDataNew . quat . to_euler ( euler . x , euler . y , euler . z ) ;
euler = euler - _ahrs - > get_trim ( ) ;
}
// return body axis gyro bias estimates in rad/sec
void NavEKF2_core : : getGyroBias ( Vector3f & gyroBias ) const
{
2015-11-09 20:25:44 -04:00
if ( dtEkfAvg < 1e-6 f ) {
2015-10-07 15:27:09 -03:00
gyroBias . zero ( ) ;
return ;
}
2015-11-09 20:25:44 -04:00
gyroBias = stateStruct . gyro_bias / dtEkfAvg ;
2015-10-07 15:27:09 -03:00
}
// return body axis gyro scale factor error as a percentage
void NavEKF2_core : : getGyroScaleErrorPercentage ( Vector3f & gyroScale ) const
{
if ( ! statesInitialised ) {
gyroScale . x = gyroScale . y = gyroScale . z = 0 ;
return ;
}
gyroScale . x = 100.0f / stateStruct . gyro_scale . x - 100.0f ;
gyroScale . y = 100.0f / stateStruct . gyro_scale . y - 100.0f ;
gyroScale . z = 100.0f / stateStruct . gyro_scale . z - 100.0f ;
}
// return tilt error convergence metric
void NavEKF2_core : : getTiltError ( float & ang ) const
{
ang = tiltErrFilt ;
}
2015-10-06 18:20:43 -03:00
// return the transformation matrix from XYZ (body) to NED axes
void NavEKF2_core : : getRotationBodyToNED ( Matrix3f & mat ) const
{
outputDataNew . quat . rotation_matrix ( mat ) ;
2016-08-09 15:33:51 -03:00
mat = mat * _ahrs - > get_rotation_vehicle_body_to_autopilot_body ( ) ;
2015-10-06 18:20:43 -03:00
}
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF2_core : : getQuaternion ( Quaternion & ret ) const
{
ret = outputDataNew . quat ;
}
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
2015-10-29 05:33:02 -03:00
uint32_t NavEKF2_core : : getLastYawResetAngle ( float & yawAng ) const
2015-10-06 18:20:43 -03:00
{
yawAng = yawResetAngle ;
return lastYawReset_ms ;
}
2015-10-29 01:03:51 -03:00
// return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
2015-10-29 05:36:44 -03:00
uint32_t NavEKF2_core : : getLastPosNorthEastReset ( Vector2f & pos ) const
2015-10-29 01:03:51 -03:00
{
pos = posResetNE ;
return lastPosReset_ms ;
}
2016-11-22 06:29:30 -04:00
// return the amount of vertical position change due to the last vertical position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF2_core : : getLastPosDownReset ( float & posD ) const
{
posD = posResetD ;
return lastPosResetD_ms ;
}
2015-10-29 01:03:51 -03:00
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
2015-10-29 05:36:44 -03:00
uint32_t NavEKF2_core : : getLastVelNorthEastReset ( Vector2f & vel ) const
2015-10-29 01:03:51 -03:00
{
vel = velResetNE ;
return lastVelReset_ms ;
}
2015-10-06 18:20:43 -03:00
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF2_core : : getWind ( Vector3f & wind ) const
{
wind . x = stateStruct . wind_vel . x ;
wind . y = stateStruct . wind_vel . y ;
wind . z = 0.0f ; // currently don't estimate this
}
2016-10-13 19:32:58 -03:00
// return the NED velocity of the body frame origin in m/s
2015-10-06 18:20:43 -03:00
//
void NavEKF2_core : : getVelNED ( Vector3f & vel ) const
{
2016-10-13 19:32:58 -03:00
// correct for the IMU position offset (EKF calculations are at the IMU)
vel = outputDataNew . velocity + velOffsetNED ;
2015-10-06 18:20:43 -03:00
}
2019-02-21 13:33:38 -04:00
// Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s
2015-10-15 02:27:40 -03:00
float NavEKF2_core : : getPosDownDerivative ( void ) const
2015-10-12 03:29:13 -03:00
{
2016-10-13 19:32:58 -03:00
// return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
// correct for the IMU offset (EKF calculations are at the IMU)
2019-09-22 02:09:58 -03:00
return vertCompFiltState . vel + velOffsetNED . z ;
2015-10-12 03:29:13 -03:00
}
2015-10-06 18:20:43 -03:00
// This returns the specific forces in the NED frame
void NavEKF2_core : : getAccelNED ( Vector3f & accelNED ) const {
accelNED = velDotNED ;
accelNED . z - = GRAVITY_MSS ;
}
// return the Z-accel bias estimate in m/s^2
void NavEKF2_core : : getAccelZBias ( float & zbias ) const {
2015-11-09 20:25:44 -04:00
if ( dtEkfAvg > 0 ) {
zbias = stateStruct . accel_zbias / dtEkfAvg ;
2015-10-06 18:20:43 -03:00
} else {
zbias = 0 ;
}
}
2016-10-13 19:32:58 -03:00
// Write the last estimated NE position of the body frame origin relative to the reference point (m).
2016-07-10 05:43:28 -03:00
// Return true if the estimate is valid
bool NavEKF2_core : : getPosNE ( Vector2f & posNE ) const
{
// There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
if ( PV_AidingMode ! = AID_NONE ) {
// This is the normal mode of operation where we can use the EKF position states
2016-10-13 19:32:58 -03:00
// correct for the IMU offset (EKF calculations are at the IMU)
posNE . x = outputDataNew . position . x + posOffsetNED . x ;
posNE . y = outputDataNew . position . y + posOffsetNED . y ;
2016-07-10 05:43:28 -03:00
return true ;
2016-10-13 19:32:58 -03:00
2016-07-10 05:43:28 -03:00
} else {
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
if ( validOrigin ) {
2017-12-01 21:02:33 -04:00
if ( ( AP : : gps ( ) . status ( ) > = AP_GPS : : GPS_OK_FIX_2D ) ) {
2016-07-10 05:43:28 -03:00
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
2017-12-01 21:02:33 -04:00
const struct Location & gpsloc = AP : : gps ( ) . location ( ) ;
2019-04-08 10:16:20 -03:00
const Vector2f tempPosNE = EKF_origin . get_distance_NE ( gpsloc ) ;
2016-07-10 05:43:28 -03:00
posNE . x = tempPosNE . x ;
posNE . y = tempPosNE . y ;
return false ;
2016-10-25 17:54:29 -03:00
} else if ( rngBcnAlignmentStarted ) {
// If we are attempting alignment using range beacon data, then report the position
posNE . x = receiverPos . x ;
posNE . y = receiverPos . y ;
return false ;
2016-07-10 05:43:28 -03:00
} else {
// If no GPS fix is available, all we can do is provide the last known position
posNE . x = outputDataNew . position . x ;
posNE . y = outputDataNew . position . y ;
return false ;
}
} else {
// If the origin has not been set, then we have no means of providing a relative position
posNE . x = 0.0f ;
posNE . y = 0.0f ;
return false ;
}
}
return false ;
}
2017-05-30 22:22:46 -03:00
// Write the last calculated D position of the body frame origin relative to the EKF origin (m).
2016-10-13 19:32:58 -03:00
// Return true if the estimate is valid
2016-07-10 05:43:28 -03:00
bool NavEKF2_core : : getPosD ( float & posD ) const
{
// The EKF always has a height estimate regardless of mode of operation
2017-07-15 21:36:57 -03:00
// Correct for the IMU offset in body frame (EKF calculations are at the IMU)
// Also correct for changes to the origin height
if ( ( frontend - > _originHgtMode & ( 1 < < 2 ) ) = = 0 ) {
2017-05-30 22:22:46 -03:00
// Any sensor height drift corrections relative to the WGS-84 reference are applied to the origin.
posD = outputDataNew . position . z + posOffsetNED . z ;
} else {
// The origin height is static and corrections are applied to the local vertical position
// so that height returned by getLLH() = height returned by getOriginLLH - posD
posD = outputDataNew . position . z + posOffsetNED . z + 0.01f * ( float ) EKF_origin . alt - ( float ) ekfGpsRefHgt ;
}
2015-10-06 18:20:43 -03:00
2016-07-10 05:43:28 -03:00
// Return the current height solution status
2016-07-10 10:15:46 -03:00
return filterStatus . flags . vert_pos ;
2016-07-10 05:43:28 -03:00
}
2017-05-30 22:22:46 -03:00
2016-10-13 19:32:58 -03:00
// return the estimated height of body frame origin above ground level
2015-10-06 18:20:43 -03:00
bool NavEKF2_core : : getHAGL ( float & HAGL ) const
{
2016-10-13 19:32:58 -03:00
HAGL = terrainState - outputDataNew . position . z - posOffsetNED . z ;
2015-10-06 18:20:43 -03:00
// If we know the terrain offset and altitude, then we have a valid height above ground estimate
return ! hgtTimeout & & gndOffsetValid & & healthy ( ) ;
}
2016-10-13 19:32:58 -03:00
// Return the last calculated latitude, longitude and height of the body frame origin in WGS-84
2015-10-06 18:20:43 -03:00
// If a calculated location isn't available, return a raw GPS measurement
// The status will return true if a calculation or raw measurement is available
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
bool NavEKF2_core : : getLLH ( struct Location & loc ) const
{
2017-12-01 21:02:33 -04:00
const AP_GPS & gps = AP : : gps ( ) ;
2018-09-13 17:39:21 -03:00
Location origin ;
float posD ;
2017-12-01 21:02:33 -04:00
2018-09-13 17:39:21 -03:00
if ( getPosD ( posD ) & & getOriginLLH ( origin ) ) {
2015-10-06 18:20:43 -03:00
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
2018-09-13 17:39:21 -03:00
loc . alt = origin . alt - posD * 100 ;
2019-01-01 20:07:57 -04:00
loc . relative_alt = 0 ;
loc . terrain_alt = 0 ;
2015-10-06 18:20:43 -03:00
// there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding)
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . horiz_pos_abs | | filterStatus . flags . horiz_pos_rel ) {
2015-10-06 18:20:43 -03:00
loc . lat = EKF_origin . lat ;
loc . lng = EKF_origin . lng ;
2016-10-13 19:32:58 -03:00
// correct for IMU offset (EKF calculations are at the IMU position)
2019-02-24 20:16:24 -04:00
loc . offset ( ( outputDataNew . position . x + posOffsetNED . x ) , ( outputDataNew . position . y + posOffsetNED . y ) ) ;
2015-10-06 18:20:43 -03:00
return true ;
} else {
2016-05-12 14:04:56 -03:00
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
2015-10-06 18:20:43 -03:00
// in this mode we cannot use the EKF states to estimate position so will return the best available data
2017-12-01 21:02:33 -04:00
if ( ( gps . status ( ) > = AP_GPS : : GPS_OK_FIX_2D ) ) {
2015-10-06 18:20:43 -03:00
// we have a GPS position fix to return
2017-12-01 21:02:33 -04:00
const struct Location & gpsloc = gps . location ( ) ;
2015-10-06 18:20:43 -03:00
loc . lat = gpsloc . lat ;
loc . lng = gpsloc . lng ;
return true ;
} else {
// if no GPS fix, provide last known position before entering the mode
2016-10-13 19:32:58 -03:00
// correct for IMU offset (EKF calculations are at the IMU position)
2020-04-24 03:02:19 -03:00
loc . lat = EKF_origin . lat ;
loc . lng = EKF_origin . lng ;
2020-05-01 23:45:06 -03:00
if ( PV_AidingMode = = AID_NONE ) {
loc . offset ( ( lastKnownPositionNE . x + posOffsetNED . x ) , ( lastKnownPositionNE . y + posOffsetNED . y ) ) ;
} else {
loc . offset ( ( outputDataNew . position . x + posOffsetNED . x ) , ( outputDataNew . position . y + posOffsetNED . y ) ) ;
}
2015-10-06 18:20:43 -03:00
return false ;
}
}
} else {
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw
// GPS reading if available and return false
2017-12-01 21:02:33 -04:00
if ( ( gps . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) ) {
const struct Location & gpsloc = gps . location ( ) ;
2015-10-06 18:20:43 -03:00
loc = gpsloc ;
2019-01-01 20:07:57 -04:00
loc . relative_alt = 0 ;
loc . terrain_alt = 0 ;
2015-10-06 18:20:43 -03:00
}
return false ;
}
}
2015-10-07 15:27:09 -03:00
// return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void NavEKF2_core : : getEkfControlLimits ( float & ekfGndSpdLimit , float & ekfNavVelGainScaler ) const
{
if ( PV_AidingMode = = AID_RELATIVE ) {
// allow 1.0 rad/sec margin for angular motion
2015-11-27 13:11:58 -04:00
ekfGndSpdLimit = MAX ( ( frontend - > _maxFlowRate - 1.0f ) , 0.0f ) * MAX ( ( terrainState - stateStruct . position [ 2 ] ) , rngOnGnd ) ;
2015-10-07 15:27:09 -03:00
// use standard gains up to 5.0 metres height and reduce above that
2015-11-27 13:11:58 -04:00
ekfNavVelGainScaler = 4.0f / MAX ( ( terrainState - stateStruct . position [ 2 ] ) , 4.0f ) ;
2015-10-07 15:27:09 -03:00
} else {
ekfGndSpdLimit = 400.0f ; //return 80% of max filter speed
ekfNavVelGainScaler = 1.0f ;
}
}
// return the LLH location of the filters NED origin
bool NavEKF2_core : : getOriginLLH ( struct Location & loc ) const
{
if ( validOrigin ) {
loc = EKF_origin ;
2017-05-09 03:30:58 -03:00
// report internally corrected reference height if enabled
2017-07-15 21:36:57 -03:00
if ( ( frontend - > _originHgtMode & ( 1 < < 2 ) ) = = 0 ) {
2017-05-09 03:30:58 -03:00
loc . alt = ( int32_t ) ( 100.0f * ( float ) ekfGpsRefHgt ) ;
}
2015-10-07 15:27:09 -03:00
}
return validOrigin ;
}
2015-10-06 18:20:43 -03:00
// return earth magnetic field estimates in measurement units / 1000
void NavEKF2_core : : getMagNED ( Vector3f & magNED ) const
{
magNED = stateStruct . earth_magfield * 1000.0f ;
}
// return body magnetic field estimates in measurement units / 1000
void NavEKF2_core : : getMagXYZ ( Vector3f & magXYZ ) const
{
magXYZ = stateStruct . body_magfield * 1000.0f ;
}
2016-03-29 16:56:16 -03:00
// return magnetometer offsets
// return true if offsets are valid
2016-03-29 17:06:42 -03:00
bool NavEKF2_core : : getMagOffsets ( uint8_t mag_idx , Vector3f & magOffsets ) const
2016-03-29 16:56:16 -03:00
{
2017-07-31 16:22:15 -03:00
if ( ! _ahrs - > get_compass ( ) ) {
return false ;
}
2016-06-16 07:41:15 -03:00
// compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
// primary compass is valid and state variances have converged
const float maxMagVar = 5E-6 f ;
bool variancesConverged = ( P [ 19 ] [ 19 ] < maxMagVar ) & & ( P [ 20 ] [ 20 ] < maxMagVar ) & & ( P [ 21 ] [ 21 ] < maxMagVar ) ;
if ( ( mag_idx = = magSelectIndex ) & &
finalInflightMagInit & &
! inhibitMagStates & &
_ahrs - > get_compass ( ) - > healthy ( magSelectIndex ) & &
variancesConverged ) {
2016-03-29 16:56:16 -03:00
magOffsets = _ahrs - > get_compass ( ) - > get_offsets ( magSelectIndex ) - stateStruct . body_magfield * 1000.0f ;
return true ;
} else {
magOffsets = _ahrs - > get_compass ( ) - > get_offsets ( magSelectIndex ) ;
return false ;
}
}
2015-11-08 05:42:09 -04:00
// return the index for the active magnetometer
uint8_t NavEKF2_core : : getActiveMag ( ) const
{
return ( uint8_t ) magSelectIndex ;
}
2015-10-06 18:20:43 -03:00
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void NavEKF2_core : : getInnovations ( Vector3f & velInnov , Vector3f & posInnov , Vector3f & magInnov , float & tasInnov , float & yawInnov ) const
{
velInnov . x = innovVelPos [ 0 ] ;
velInnov . y = innovVelPos [ 1 ] ;
velInnov . z = innovVelPos [ 2 ] ;
posInnov . x = innovVelPos [ 3 ] ;
posInnov . y = innovVelPos [ 4 ] ;
posInnov . z = innovVelPos [ 5 ] ;
magInnov . x = 1e3 f * innovMag [ 0 ] ; // Convert back to sensor units
magInnov . y = 1e3 f * innovMag [ 1 ] ; // Convert back to sensor units
magInnov . z = 1e3 f * innovMag [ 2 ] ; // Convert back to sensor units
tasInnov = innovVtas ;
yawInnov = innovYaw ;
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
// this indicates the amount of margin available when tuning the various error traps
2015-10-29 02:06:24 -03:00
// also return the delta in position due to the last position reset
2015-10-06 18:20:43 -03:00
void NavEKF2_core : : getVariances ( float & velVar , float & posVar , float & hgtVar , Vector3f & magVar , float & tasVar , Vector2f & offset ) const
{
velVar = sqrtf ( velTestRatio ) ;
posVar = sqrtf ( posTestRatio ) ;
hgtVar = sqrtf ( hgtTestRatio ) ;
2015-10-28 22:36:53 -03:00
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
2015-11-27 13:11:58 -04:00
magVar . x = sqrtf ( MAX ( magTestRatio . x , yawTestRatio ) ) ;
magVar . y = sqrtf ( MAX ( magTestRatio . y , yawTestRatio ) ) ;
magVar . z = sqrtf ( MAX ( magTestRatio . z , yawTestRatio ) ) ;
2015-10-06 18:20:43 -03:00
tasVar = sqrtf ( tasTestRatio ) ;
2015-10-29 02:06:24 -03:00
offset = posResetNE ;
2015-10-06 18:20:43 -03:00
}
/*
return the filter fault status as a bitmasked integer
0 = quaternions are NaN
1 = velocities are NaN
2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion
2020-06-29 08:10:41 -03:00
4 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion
2015-10-06 18:20:43 -03:00
7 = filter is not initialised
*/
2016-05-10 03:00:36 -03:00
void NavEKF2_core : : getFilterFaults ( uint16_t & faults ) const
2015-10-06 18:20:43 -03:00
{
faults = ( stateStruct . quat . is_nan ( ) < < 0 |
stateStruct . velocity . is_nan ( ) < < 1 |
faultStatus . bad_xmag < < 2 |
faultStatus . bad_ymag < < 3 |
faultStatus . bad_zmag < < 4 |
faultStatus . bad_airspeed < < 5 |
faultStatus . bad_sideslip < < 6 |
! statesInitialised < < 7 ) ;
}
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
4 = true airspeed measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
*/
void NavEKF2_core : : getFilterTimeouts ( uint8_t & timeouts ) const
{
timeouts = ( posTimeout < < 0 |
velTimeout < < 1 |
hgtTimeout < < 2 |
magTimeout < < 3 |
tasTimeout < < 4 ) ;
}
2016-07-10 10:15:46 -03:00
// Return the navigation filter status message
2015-10-06 18:20:43 -03:00
void NavEKF2_core : : getFilterStatus ( nav_filter_status & status ) const
{
2016-07-10 10:15:46 -03:00
status = filterStatus ;
2015-10-07 21:38:26 -03:00
}
/*
return filter gps quality check status
*/
void NavEKF2_core : : getFilterGpsStatus ( nav_gps_status & faults ) const
{
// init return value
faults . value = 0 ;
// set individual flags
2015-10-09 14:59:47 -03:00
faults . flags . bad_sAcc = gpsCheckStatus . bad_sAcc ; // reported speed accuracy is insufficient
faults . flags . bad_hAcc = gpsCheckStatus . bad_hAcc ; // reported horizontal position accuracy is insufficient
2017-02-17 18:09:16 -04:00
faults . flags . bad_vAcc = gpsCheckStatus . bad_vAcc ; // reported vertical position accuracy is insufficient
2015-10-09 14:59:47 -03:00
faults . flags . bad_yaw = gpsCheckStatus . bad_yaw ; // EKF heading accuracy is too large for GPS use
faults . flags . bad_sats = gpsCheckStatus . bad_sats ; // reported number of satellites is insufficient
faults . flags . bad_horiz_drift = gpsCheckStatus . bad_horiz_drift ; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static)
faults . flags . bad_hdop = gpsCheckStatus . bad_hdop ; // reported HDoP is too large to start using GPS
faults . flags . bad_vert_vel = gpsCheckStatus . bad_vert_vel ; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static)
faults . flags . bad_fix = gpsCheckStatus . bad_fix ; // The GPS cannot provide the 3D fix required
faults . flags . bad_horiz_vel = gpsCheckStatus . bad_horiz_vel ; // The GPS horizontal speed is excessive (check assumes the vehicle is static)
2015-10-06 18:20:43 -03:00
}
// send an EKF_STATUS message to GCS
2019-12-10 03:03:55 -04:00
void NavEKF2_core : : send_status_report ( mavlink_channel_t chan ) const
2015-10-06 18:20:43 -03:00
{
// prepare flags
uint16_t flags = 0 ;
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . attitude ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_ATTITUDE ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . horiz_vel ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_VELOCITY_HORIZ ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . vert_vel ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_VELOCITY_VERT ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . horiz_pos_rel ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_POS_HORIZ_REL ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . horiz_pos_abs ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_POS_HORIZ_ABS ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . vert_pos ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_POS_VERT_ABS ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . terrain_alt ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_POS_VERT_AGL ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . const_pos_mode ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_CONST_POS_MODE ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . pred_horiz_pos_rel ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_PRED_POS_HORIZ_REL ;
}
2016-07-10 10:15:46 -03:00
if ( filterStatus . flags . pred_horiz_pos_abs ) {
2015-10-06 18:20:43 -03:00
flags | = EKF_PRED_POS_HORIZ_ABS ;
}
2020-02-07 16:33:40 -04:00
if ( ! filterStatus . flags . initalized ) {
flags | = EKF_UNINITIALIZED ;
}
2015-10-06 18:20:43 -03:00
// get variances
float velVar , posVar , hgtVar , tasVar ;
Vector3f magVar ;
Vector2f offset ;
getVariances ( velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
2020-04-04 02:37:11 -03:00
const float mag_max = fmaxf ( fmaxf ( magVar . x , magVar . y ) , magVar . z ) ;
2016-10-06 17:58:20 -03:00
// Only report range finder normalised innovation levels if the EKF needs the data for primary
// height estimation or optical flow operation. This prevents false alarms at the GCS if a
// range finder is fitted for other applications
2016-09-19 21:42:06 -03:00
float temp ;
2017-12-13 19:01:53 -04:00
if ( ( ( frontend - > _useRngSwHgt > 0 ) & & activeHgtSource = = HGT_SOURCE_RNG ) | | ( PV_AidingMode = = AID_RELATIVE & & flowDataValid ) ) {
2016-09-19 21:42:06 -03:00
temp = sqrtf ( auxRngTestRatio ) ;
} else {
temp = 0.0f ;
}
2015-10-06 18:20:43 -03:00
// send message
2020-04-04 02:37:11 -03:00
mavlink_msg_ekf_status_report_send ( chan , flags , velVar , posVar , hgtVar , mag_max , temp , tasVar ) ;
2015-10-06 18:20:43 -03:00
}
2015-10-30 00:59:14 -03:00
// report the reason for why the backend is refusing to initialise
const char * NavEKF2_core : : prearm_failure_reason ( void ) const
{
2019-07-01 19:49:48 -03:00
if ( gpsGoodToAlign ) {
2015-10-30 00:59:14 -03:00
// we are not failing
return nullptr ;
}
return prearm_fail_string ;
}
2015-11-09 20:25:44 -04:00
// report the number of frames lapsed since the last state prediction
// this is used by other instances to level load
uint8_t NavEKF2_core : : getFramesSincePredict ( void ) const
{
return framesSincePredict ;
}
2016-06-29 02:57:10 -03:00
// publish output observer angular, velocity and position tracking error
void NavEKF2_core : : getOutputTrackingError ( Vector3f & error ) const
{
error = outputTrackError ;
}
2019-07-24 06:48:07 -03:00
// return true when external nav data is also being used as a yaw observation
bool NavEKF2_core : : isExtNavUsedForYaw ( )
{
return extNavUsedForYaw ;
}
2020-04-22 21:45:34 -03:00
bool NavEKF2_core : : getDataEKFGSF ( float & yaw_composite , float & yaw_composite_variance , float yaw [ N_MODELS_EKFGSF ] , float innov_VN [ N_MODELS_EKFGSF ] , float innov_VE [ N_MODELS_EKFGSF ] , float weight [ N_MODELS_EKFGSF ] )
2020-03-25 00:53:13 -03:00
{
if ( yawEstimator ! = nullptr ) {
return yawEstimator - > getLogData ( yaw_composite , yaw_composite_variance , yaw , innov_VN , innov_VE , weight ) ;
}
return false ;
2020-06-29 08:10:41 -03:00
}