diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 89e810cae6d96..f635c185af7cb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -374,7 +374,7 @@ void NavEKF3_core::readMagData() * Downsampling is done using a method that does not introduce coning or sculling * errors. */ -void NavEKF3_core::readIMUData() +void NavEKF3_core::readIMUData(bool startPredictEnabled) { const auto &ins = dal.ins(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 67298272ffe85..1928c73c169b6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -475,7 +475,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) } // read all the sensors required to start the EKF the states - readIMUData(); + readIMUData(false); // don't allow prediction readMagData(); readGpsData(); readGpsYawData(); @@ -617,9 +617,6 @@ void NavEKF3_core::CovarianceInit() // Update Filter States - this should be called whenever new IMU data is available void NavEKF3_core::UpdateFilter(bool predict) { - // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started - startPredictEnabled = predict; - // don't run filter updates if states have not been initialised if (!statesInitialised) { return; @@ -636,7 +633,7 @@ void NavEKF3_core::UpdateFilter(bool predict) controlFilterModes(); // read IMU data as delta angles and velocities - readIMUData(); + readIMUData(predict); // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer if (runUpdates) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 322ebc6907ab2..94749ded525c3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -766,7 +766,7 @@ class NavEKF3_core : public NavEKF_core_common void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index); // update IMU delta angle and delta velocity measurements - void readIMUData(); + void readIMUData(bool startPredictEnabled); // update estimate of inactive bias states void learnInactiveBiases(); @@ -1191,7 +1191,6 @@ class NavEKF3_core : public NavEKF_core_common uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF bool runUpdates; // boolean true when the EKF updates can be run uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction - bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycle uint8_t localFilterTimeStep_ms; // average number of msec between filter updates ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2) Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)