Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : 
       3             : #include "AP_NavEKF2.h"
       4             : #include "AP_NavEKF2_core.h"
       5             : #include <AP_DAL/AP_DAL.h>
       6             : #include <AP_AHRS/AP_AHRS.h>
       7             : #include <GCS_MAVLink/GCS.h>
       8             : 
       9             : extern const AP_HAL::HAL& hal;
      10             : 
      11             : 
      12             : // Check basic filter health metrics and return a consolidated health status
      13    10967631 : bool NavEKF2_core::healthy(void) const
      14             : {
      15             :     uint16_t faultInt;
      16    10967631 :     getFilterFaults(faultInt);
      17    10967631 :     if (faultInt > 0) {
      18             :         return false;
      19             :     }
      20    10967631 :     if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
      21             :         // all three metrics being above 1 means the filter is
      22             :         // extremely unhealthy.
      23             :         return false;
      24             :     }
      25             :     // Give the filter a second to settle before use
      26    10961435 :     if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
      27             :         return false;
      28             :     }
      29             :     // position and height innovations must be within limits when on-ground and in a static mode of operation
      30    10870619 :     ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
      31    10870619 :     if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
      32       45413 :         return false;
      33             :     }
      34             : 
      35             :     // all OK
      36             :     return true;
      37             : }
      38             : 
      39             : // Return a consolidated error score where higher numbers represent larger errors
      40             : // Intended to be used by the front-end to determine which is the primary EKF
      41     2214166 : ftype NavEKF2_core::errorScore() const
      42             : {
      43     2214166 :     ftype score = 0.0f;
      44     2214166 :     if (tiltAlignComplete && yawAlignComplete) {
      45             :         // Check GPS fusion performance
      46     2143573 :         score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
      47             :         // Check altimeter fusion performance
      48     2143573 :         score = MAX(score, hgtTestRatio);
      49             :         // Check attitude corrections
      50             :         const ftype tiltErrThreshold = 0.05f;
      51     2143573 :         score = MAX(score, tiltErrFilt / tiltErrThreshold);
      52             :     }
      53     2214166 :     return score;
      54             : }
      55             : 
      56             : // provides the height limit to be observed by the control loops
      57             : // returns false if no height limiting is required
      58             : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
      59     2210449 : bool NavEKF2_core::getHeightControlLimit(float &height) const
      60             : {
      61             :     // only ask for limiting if we are doing optical flow only navigation
      62     2210449 :     if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
      63             :         // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
      64             : #if AP_RANGEFINDER_ENABLED
      65           0 :         const auto *_rng = dal.rangefinder();
      66           0 :         if (_rng == nullptr) {
      67             :             // we really, really shouldn't be here.
      68             :             return false;
      69             :         }
      70           0 :         height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f);
      71             : #else
      72             :         return false;
      73             : #endif
      74             :         // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
      75           0 :         if (frontend->_altSource != 1) {
      76           0 :             height -= terrainState;
      77             :         }
      78           0 :         return true;
      79             :     } else {
      80             :         return false;
      81             :     }
      82             : }
      83             : 
      84             : 
      85             : // return the Euler roll, pitch and yaw angle in radians
      86     3777545 : void NavEKF2_core::getEulerAngles(Vector3f &euler) const
      87             : {
      88     3777545 :     outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
      89     3777545 :     euler = euler - dal.get_trim();
      90     3777545 : }
      91             : 
      92             : // return body axis gyro bias estimates in rad/sec
      93     3777861 : void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
      94             : {
      95     3777861 :     if (dtEkfAvg < 1e-6f) {
      96      178978 :         gyroBias.zero();
      97      178978 :         return;
      98             :     }
      99     3598883 :     gyroBias = stateStruct.gyro_bias.tofloat() / dtEkfAvg;
     100             : }
     101             : 
     102             : // return body axis gyro scale factor error as a percentage
     103     1567096 : void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
     104             : {
     105     1567096 :     if (!statesInitialised) {
     106       83878 :         gyroScale.x = gyroScale.y = gyroScale.z = 0;
     107       83878 :         return;
     108             :     }
     109     1483218 :     gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
     110     1483218 :     gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
     111     1483218 :     gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f;
     112             : }
     113             : 
     114             : // return the transformation matrix from XYZ (body) to NED axes
     115     2210908 : void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const
     116             : {
     117     2210908 :     outputDataNew.quat.rotation_matrix(mat);
     118     2210908 :     mat = mat * dal.get_rotation_vehicle_body_to_autopilot_body();
     119     2210908 : }
     120             : 
     121             : // return the quaternions defining the rotation from NED to XYZ (body) axes
     122     3777545 : void NavEKF2_core::getQuaternion(Quaternion& ret) const
     123             : {
     124     3777545 :     ret = outputDataNew.quat.tofloat();
     125     3777545 : }
     126             : 
     127             : // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
     128     3777545 : void NavEKF2_core::getWind(Vector3f &wind) const
     129             : {
     130     3777545 :     wind.x = stateStruct.wind_vel.x;
     131     3777545 :     wind.y = stateStruct.wind_vel.y;
     132     3777545 :     wind.z = 0.0f; // currently don't estimate this
     133     3777545 : }
     134             : 
     135             : 
     136             : // return the NED velocity of the body frame origin in m/s
     137             : //
     138     3777545 : void NavEKF2_core::getVelNED(Vector3f &vel) const
     139             : {
     140             :     // correct for the IMU position offset (EKF calculations are at the IMU)
     141     3777545 :     vel = (outputDataNew.velocity + velOffsetNED).tofloat();
     142     3777545 : }
     143             : 
     144             : // return estimate of true airspeed vector in body frame in m/s
     145             : // returns false if estimate is unavailable
     146      135980 : bool NavEKF2_core::getAirSpdVec(Vector3f &vel) const
     147             : {
     148      135980 :     if (PV_AidingMode == AID_NONE) {
     149             :         return false;
     150             :     }
     151       91865 :     vel = (outputDataNew.velocity + velOffsetNED).tofloat();
     152       91865 :     if (!inhibitWindStates) {
     153         686 :         vel.x -= stateStruct.wind_vel.x;
     154         686 :         vel.y -= stateStruct.wind_vel.y;
     155             :     }
     156       91865 :     Matrix3f Tnb; // rotation from nav to body frame
     157       91865 :     outputDataNew.quat.inverse().rotation_matrix(Tnb);
     158       91865 :     vel = Tnb * vel;
     159       91865 :     return true;
     160             : }
     161             : 
     162             : // Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s
     163     3777545 : float NavEKF2_core::getPosDownDerivative(void) const
     164             : {
     165             :     // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
     166             :     // correct for the IMU offset (EKF calculations are at the IMU)
     167     3777545 :     return vertCompFiltState.vel + velOffsetNED.z;
     168             : }
     169             : 
     170             : // return the Z-accel bias estimate in m/s^2
     171     3777545 : void NavEKF2_core::getAccelZBias(float &zbias) const {
     172     3777545 :     if (dtEkfAvg > 0) {
     173     3598567 :         zbias = stateStruct.accel_zbias / dtEkfAvg;
     174             :     } else {
     175      178978 :         zbias = 0;
     176             :     }
     177     3777545 : }
     178             : 
     179             : // Write the last estimated NE position of the body frame origin relative to the reference point (m).
     180             : // Return true if the estimate is valid
     181     3777545 : bool NavEKF2_core::getPosNE(Vector2p &posNE) const
     182             : {
     183             :     // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
     184     3777545 :     if (PV_AidingMode != AID_NONE) {
     185             :         // This is the normal mode of operation where we can use the EKF position states
     186             :         // correct for the IMU offset (EKF calculations are at the IMU)
     187     3085178 :         posNE.x = outputDataNew.position.x + posOffsetNED.x;
     188     3085178 :         posNE.y = outputDataNew.position.y + posOffsetNED.y;
     189     3085178 :         return true;
     190             : 
     191             :     } else {
     192             :         // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
     193      692367 :         if(validOrigin) {
     194      395054 :             if ((dal.gps().status(dal.gps().primary_sensor()) >= AP_GPS_FixType::FIX_2D)) {
     195             :                 // If the origin has been set and we have GPS, then return the GPS position relative to the origin
     196      391644 :                 const Location &gpsloc = dal.gps().location();
     197      391644 :                 posNE = EKF_origin.get_distance_NE_postype(gpsloc);
     198      391644 :                 return false;
     199        3410 :             } else if (rngBcnAlignmentStarted) {
     200             :                 // If we are attempting alignment using range beacon data, then report the position
     201           0 :                 posNE.x = receiverPos.x;
     202           0 :                 posNE.y = receiverPos.y;
     203           0 :                 return false;
     204             :             } else {
     205             :                 // If no GPS fix is available, all we can do is provide the last known position
     206        3410 :                 posNE.x = outputDataNew.position.x;
     207        3410 :                 posNE.y = outputDataNew.position.y;
     208        3410 :                 return false;
     209             :             }
     210             :         } else {
     211             :             // If the origin has not been set, then we have no means of providing a relative position
     212      297313 :             posNE.x = 0.0f;
     213      297313 :             posNE.y = 0.0f;
     214      297313 :             return false;
     215             :         }
     216             :     }
     217             :     return false;
     218             : }
     219             : 
     220             : // Write the last calculated D position of the body frame origin relative to the EKF origin (m).
     221             : // Return true if the estimate is valid
     222     5987994 : bool NavEKF2_core::getPosD(postype_t &posD) const
     223             : {
     224             :     // The EKF always has a height estimate regardless of mode of operation
     225             :     // Correct for the IMU offset in body frame (EKF calculations are at the IMU)
     226             :     // Also correct for changes to the origin height
     227     5987994 :     if ((frontend->_originHgtMode & (1<<2)) == 0) {
     228             :         // Any sensor height drift corrections relative to the WGS-84 reference are applied to the origin.
     229     5987994 :         posD = outputDataNew.position.z + posOffsetNED.z;
     230             :     } else {
     231             :         // The origin height is static and corrections are applied to the local vertical position
     232             :         // so that height returned by getLLH() = height returned by getOriginLLH - posD
     233           0 :         posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
     234             :     }
     235             : 
     236             :     // Return the current height solution status
     237     5987994 :     return filterStatus.flags.vert_pos;
     238             : 
     239             : }
     240             : 
     241             : // return the estimated height of body frame origin above ground level
     242     2210449 : bool NavEKF2_core::getHAGL(float &HAGL) const
     243             : {
     244     2210449 :     HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
     245             :     // If we know the terrain offset and altitude, then we have a valid height above ground estimate
     246     2210449 :     return !hgtTimeout && gndOffsetValid && healthy();
     247             : }
     248             : 
     249             : 
     250             : // Return the last calculated latitude, longitude and height of the body frame origin in WGS-84
     251             : // If a calculated location isn't available, return a raw GPS measurement
     252             : // The status will return true if a calculation or raw measurement is available
     253             : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     254     2210449 : bool NavEKF2_core::getLLH(Location &loc) const
     255             : {
     256     2210449 :     const auto &gps = dal.gps();
     257     2210449 :     Location origin;
     258             :     postype_t posD;
     259             : 
     260     2210449 :     if(getPosD(posD) && getOriginLLH(origin)) {
     261             :         // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
     262     1903845 :         loc.set_alt_cm(origin.alt - posD*100, Location::AltFrame::ABSOLUTE);
     263             : 
     264             :         // there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding)
     265     1903845 :         if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
     266     1660792 :             loc.lat = EKF_origin.lat;
     267     1660792 :             loc.lng = EKF_origin.lng;
     268             :             // correct for IMU offset (EKF calculations are at the IMU position)
     269     1660792 :             loc.offset((outputDataNew.position.x + posOffsetNED.x), (outputDataNew.position.y + posOffsetNED.y));
     270     1660792 :             return true;
     271             :         } else {
     272             :             // we could be in constant position mode  because the vehicle has taken off without GPS, or has lost GPS
     273             :             // in this mode we cannot use the EKF states to estimate position so will return the best available data
     274      243053 :             if ((gps.status() >= AP_GPS_FixType::FIX_2D)) {
     275             :                 // we have a GPS position fix to return
     276      240592 :                 const Location &gpsloc = gps.location();
     277      240592 :                 loc.lat = gpsloc.lat;
     278      240592 :                 loc.lng = gpsloc.lng;
     279      240592 :                 return true;
     280             :             } else {
     281             :                 // if no GPS fix, provide last known position before entering the mode
     282             :                 // correct for IMU offset (EKF calculations are at the IMU position)
     283        2461 :                 loc.lat = EKF_origin.lat;
     284        2461 :                 loc.lng = EKF_origin.lng;
     285        2461 :                 if (PV_AidingMode == AID_NONE) {
     286        1628 :                     loc.offset((lastKnownPositionNE.x + posOffsetNED.x), (lastKnownPositionNE.y + posOffsetNED.y));
     287             :                 } else {
     288         833 :                     loc.offset((outputDataNew.position.x + posOffsetNED.x), (outputDataNew.position.y + posOffsetNED.y));
     289             :                 }
     290        2461 :                 return false;
     291             :             }
     292             :         }
     293             :     } else {
     294             :         // If no origin has been defined for the EKF, then we cannot use its position states so return a raw
     295             :         // GPS reading if available and return false
     296      306604 :         if ((gps.status() >= AP_GPS_FixType::FIX_3D)) {
     297      171049 :             loc = gps.location();
     298             :         }
     299      306604 :         return false;
     300             :     }
     301             : }
     302             : 
     303             : 
     304             : // return the horizontal speed limit in m/s set by optical flow sensor limits
     305             : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
     306     2210449 : void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
     307             : {
     308     2210449 :     if (PV_AidingMode == AID_RELATIVE) {
     309             :         // allow 1.0 rad/sec margin for angular motion
     310           0 :         ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd);
     311             :         // use standard gains up to 5.0 metres height and reduce above that
     312           0 :         ekfNavVelGainScaler = 4.0f / MAX((terrainState - stateStruct.position[2]),4.0f);
     313             :     } else {
     314     2210449 :         ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
     315     2210449 :         ekfNavVelGainScaler = 1.0f;
     316             :     }
     317     2210449 : }
     318             : 
     319             : 
     320             : // return the LLH location of the filters NED origin
     321     3898482 : bool NavEKF2_core::getOriginLLH(Location &loc) const
     322             : {
     323     3898482 :     if (validOrigin) {
     324     3291795 :         loc = EKF_origin;
     325             :         // report internally corrected reference height if enabled
     326     3291795 :         if ((frontend->_originHgtMode & (1<<2)) == 0) {
     327     3291795 :             loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);
     328             :         }
     329             :     }
     330     3898482 :     return validOrigin;
     331             : }
     332             : 
     333             : // return earth magnetic field estimates in measurement units / 1000
     334     3777545 : void NavEKF2_core::getMagNED(Vector3f &magNED) const
     335             : {
     336     3777545 :     magNED = (stateStruct.earth_magfield * 1000.0).tofloat();
     337     3777545 : }
     338             : 
     339             : // return body magnetic field estimates in measurement units / 1000
     340     3777545 : void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
     341             : {
     342     3777545 :     magXYZ = (stateStruct.body_magfield*1000.0).tofloat();
     343     3777545 : }
     344             : 
     345             : // return magnetometer offsets
     346             : // return true if offsets are valid
     347           0 : bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
     348             : {
     349           0 :     const auto &compass = dal.compass();
     350           0 :     if (!compass.available()) {
     351             :         return false;
     352             :     }
     353             : 
     354             :     // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
     355             :     // primary compass is valid and state variances have converged
     356             :     const ftype maxMagVar = 5E-6f;
     357           0 :     bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
     358           0 :     if ((mag_idx == magSelectIndex) &&
     359           0 :             finalInflightMagInit &&
     360           0 :             !inhibitMagStates &&
     361           0 :             compass.healthy(magSelectIndex) &&
     362             :             variancesConverged) {
     363           0 :         magOffsets = compass.get_offsets(magSelectIndex) - (stateStruct.body_magfield*1000.0).tofloat();
     364           0 :         return true;
     365             :     } else {
     366           0 :         magOffsets = compass.get_offsets(magSelectIndex);
     367           0 :         return false;
     368             :     }
     369             : }
     370             : 
     371             : // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
     372     1570504 : bool NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
     373             : {
     374     1570504 :     velInnov.x = innovVelPos[0];
     375     1570504 :     velInnov.y = innovVelPos[1];
     376     1570504 :     velInnov.z = innovVelPos[2];
     377     1570504 :     posInnov.x = innovVelPos[3];
     378     1570504 :     posInnov.y = innovVelPos[4];
     379     1570504 :     posInnov.z = innovVelPos[5];
     380     1570504 :     magInnov.x = 1e3f*innovMag[0]; // Convert back to sensor units
     381     1570504 :     magInnov.y = 1e3f*innovMag[1]; // Convert back to sensor units
     382     1570504 :     magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units
     383     1570504 :     tasInnov   = innovVtas;
     384     1570504 :     yawInnov   = innovYaw;
     385             : 
     386     1570504 :     return true;
     387             : }
     388             : 
     389             : // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
     390             : // this indicates the amount of margin available when tuning the various error traps
     391             : // also return the delta in position due to the last position reset
     392     3777545 : bool NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
     393             : {
     394     3777545 :     velVar   = sqrtF(velTestRatio);
     395     3777545 :     posVar   = sqrtF(posTestRatio);
     396     3777545 :     hgtVar   = sqrtF(hgtTestRatio);
     397             :     // If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
     398     3777545 :     magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
     399     3777545 :     magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
     400     3777545 :     magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
     401     3777545 :     tasVar   = sqrtF(tasTestRatio);
     402     3777545 :     offset   = posResetNE.tofloat();
     403             : 
     404     3777545 :     return true;
     405             : }
     406             : 
     407             : 
     408             : /*
     409             : return the filter fault status as a bitmasked integer
     410             :  0 = quaternions are NaN
     411             :  1 = velocities are NaN
     412             :  2 = badly conditioned X magnetometer fusion
     413             :  3 = badly conditioned Y magnetometer fusion
     414             :  4 = badly conditioned Z magnetometer fusion
     415             :  5 = badly conditioned airspeed fusion
     416             :  6 = badly conditioned synthetic sideslip fusion
     417             :  7 = filter is not initialised
     418             : */
     419    14745176 : void  NavEKF2_core::getFilterFaults(uint16_t &faults) const
     420             : {
     421    14745176 :     faults = (stateStruct.quat.is_nan()     * uint16_t(NavFilterFaultBit::BAD_QUATERNION) |
     422    14745176 :               stateStruct.velocity.is_nan() * uint16_t(NavFilterFaultBit::BAD_VELOCITY) |
     423    14745176 :               faultStatus.bad_xmag          * uint16_t(NavFilterFaultBit::BAD_XMAG) |
     424    14745176 :               faultStatus.bad_ymag          * uint16_t(NavFilterFaultBit::BAD_YMAG) |
     425    14745176 :               faultStatus.bad_zmag          * uint16_t(NavFilterFaultBit::BAD_ZMAG) |
     426    14745176 :               faultStatus.bad_airspeed      * uint16_t(NavFilterFaultBit::BAD_AIRSPEED) |
     427    14745176 :               faultStatus.bad_sideslip      * uint16_t(NavFilterFaultBit::BAD_SIDESLIP) |
     428    14745176 :               !statesInitialised            * uint16_t(NavFilterFaultBit::NOT_INITIALISED));
     429    14745176 : }
     430             : 
     431             : /*
     432             : return filter timeout status as a bitmasked integer
     433             :  0 = position measurement timeout
     434             :  1 = velocity measurement timeout
     435             :  2 = height measurement timeout
     436             :  3 = magnetometer measurement timeout
     437             :  4 = true airspeed measurement timeout
     438             :  5 = unassigned
     439             :  6 = unassigned
     440             :  7 = unassigned
     441             : */
     442             : // Return the navigation filter status message
     443     3777545 : void  NavEKF2_core::getFilterStatus(nav_filter_status &status) const
     444             : {
     445     3777545 :     status = filterStatus;
     446     3777545 : }
     447             : 
     448             : /*
     449             : return filter gps quality check status
     450             : */
     451     1567096 : void  NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const
     452             : {
     453             :     // init return value
     454     1567096 :     faults.value = 0;
     455             : 
     456             :     // set individual flags
     457     1567096 :     faults.flags.bad_sAcc           = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient
     458     1567096 :     faults.flags.bad_hAcc           = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient
     459     1567096 :     faults.flags.bad_vAcc           = gpsCheckStatus.bad_vAcc; // reported vertical position accuracy is insufficient
     460     1567096 :     faults.flags.bad_yaw            = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use
     461     1567096 :     faults.flags.bad_sats           = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient
     462     1567096 :     faults.flags.bad_horiz_drift    = gpsCheckStatus.bad_horiz_drift; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static)
     463     1567096 :     faults.flags.bad_hdop           = gpsCheckStatus.bad_hdop; // reported HDoP is too large to start using GPS
     464     1567096 :     faults.flags.bad_vert_vel       = gpsCheckStatus.bad_vert_vel; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static)
     465     1567096 :     faults.flags.bad_fix            = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required
     466     1567096 :     faults.flags.bad_horiz_vel      = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static)
     467     1567096 : }
     468             : 
     469             : // return a terrain altitude variance
     470     2210449 : bool NavEKF2_core::getTerrainAltVariance(float &temp) const
     471             : {
     472             :     // Only report range finder normalised innovation levels if the EKF needs the data for primary
     473             :     // height estimation or optical flow operation. This prevents false alarms at the GCS if a
     474             :     // range finder is fitted for other applications
     475     2210449 :     if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
     476           0 :         temp = sqrtF(auxRngTestRatio);
     477             :     } else {
     478     2210449 :         temp = 0.0f;
     479             :     }
     480             :     // we always successfully return a value, even if that value is a
     481             :     // "nothing to look at here" 0 value:
     482     2210449 :     return true;
     483             : }
     484             : 
     485             : // report the reason for why the backend is refusing to initialise
     486           0 : const char *NavEKF2_core::prearm_failure_reason(void) const
     487             : {
     488           0 :     if (gpsGoodToAlign) {
     489             :         // we are not failing
     490             :         return nullptr;
     491             :     }
     492           0 :     return prearm_fail_string;
     493             : }
     494             : 
     495             : 
     496             : // report the number of frames lapsed since the last state prediction
     497             : // this is used by other instances to level load
     498     4213784 : uint8_t NavEKF2_core::getFramesSincePredict(void) const
     499             : {
     500     4213784 :     return framesSincePredict;
     501             : }
     502             : 
     503             : // return true when external nav data is also being used as a yaw observation
     504     4420898 : bool NavEKF2_core::isExtNavUsedForYaw() const
     505             : {
     506     4420898 :     return extNavUsedForYaw;
     507             : }