Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_NavEKF3: do not store prediction-enabled as state #27447

Merged
merged 1 commit into from
Jul 9, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down
7 changes: 2 additions & 5 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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;
Expand All @@ -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) {
Expand Down
3 changes: 1 addition & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.h
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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)
Expand Down
Loading