diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 44734f970f..e6a9ccddfe 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -204,7 +204,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _ahrs(ahrs), _baro(baro), staticModeDemanded(true), // staticMode demanded from outside - useAirspeed(true), // activates fusion of compass data useCompass(true), // activates fusion of airspeed data covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates @@ -242,6 +241,7 @@ 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 diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index bf58feae73..d1915be27a 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -327,7 +327,7 @@ 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) - const bool useAirspeed; // boolean true if airspeed data is being used + 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