diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 84dc5faa21..4be0a11c0d 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -241,7 +241,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : hgtRate = 0.0f; mag_state.q0 = 1; mag_state.DCM.identity(); - useAirspeed = _ahrs->get_airspeed(); } bool NavEKF::healthy(void) const @@ -576,7 +575,7 @@ void NavEKF::SelectVelPosFusion() // reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives skipCounter = velPosFuseStepRatio; // If a long time since last GPS update, then reset position and velocity and reset stored state history - if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) { + if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed()) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed())) { ResetPosition(); ResetVelocity(); StoreStatesReset(); @@ -650,7 +649,7 @@ void NavEKF::SelectTasFusion() { readAirSpdData(); // Determine if data is waiting to be fused - tasDataWaiting = (statesInitialised && useAirspeed && !onGround && (tasDataWaiting || newDataTas)); + tasDataWaiting = (statesInitialised && useAirspeed() && !onGround && (tasDataWaiting || newDataTas)); bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax); // Fuse Airspeed Measurements - hold off if magnetometer fusion has been performed, unless maximum time interval exceeded if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow)) @@ -1458,7 +1457,7 @@ void NavEKF::FuseVelPosNED() // set the GPS data timeout depending on whether airspeed data is present uint32_t gpsRetryTime; - if (useAirspeed) gpsRetryTime = _gpsRetryTimeUseTAS; + if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS; else gpsRetryTime = _gpsRetryTimeNoTAS; // Form the observation vector @@ -2366,7 +2365,7 @@ void NavEKF::CopyAndFixCovariances() } // if we flying, but not using airspeed, we want all the off-diagonals for the wind // states to remain zero and want to keep the old variances for these states - else if (!useAirspeed) { + else if (!useAirspeed()) { // copy calculated variances we want to propagate for (uint8_t i=0; i<=13; i++) { P[i][i] = nextP[i][i]; @@ -2657,4 +2656,12 @@ void NavEKF::ZeroVariables() memset(&posNE[0], 0, sizeof(posNE)); } +bool NavEKF::useAirspeed(void) const +{ + if (_ahrs->get_airspeed() == NULL) { + return false; + } + return _ahrs->get_airspeed()->use(); +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index ab208076f6..d837c696b7 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -256,6 +256,8 @@ private: // reset the vertical position state using the last height measurement void ResetHeight(void); + // return true if we should use the airspeed sensor + bool useAirspeed(void) const; private: // EKF Mavlink Tuneable Parameters @@ -330,7 +332,6 @@ private: ftype dt; // time lapsed since the last covariance prediction (sec) ftype hgtRate; // state for rate of change of height filter bool onGround; // boolean true when the flight vehicle is on the ground (not flying) - bool useAirspeed; // boolean true if airspeed data is being used const bool useCompass; // boolean true if magnetometer data is being used Vector6 innovVelPos; // innovation output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements