Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: prevent high rate mag data locking out other data fusion

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-20 20:12:17 +11:00
parent ab8c28a7cc
commit cd8b9c7d26
5 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -195,8 +195,12 @@ void NavEKF2_core::SelectTasFusion()
{ {
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs // If so, don't fuse measurements on this time step to reduce frame over-runs
if (magFusePerformed && dtIMUavg < 0.005f) { // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !airSpdFusionDelayed) {
airSpdFusionDelayed = true;
return; return;
} else {
airSpdFusionDelayed = false;
} }
// get true airspeed measurement // get true airspeed measurement
@ -265,8 +269,12 @@ void NavEKF2_core::SelectBetaFusion()
{ {
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs // If so, don't fuse measurements on this time step to reduce frame over-runs
if (magFusePerformed && dtIMUavg < 0.005f) { // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !sideSlipFusionDelayed) {
sideSlipFusionDelayed = true;
return; return;
} else {
sideSlipFusionDelayed = false;
} }
// set true when the fusion time interval has triggered // set true when the fusion time interval has triggered

6
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -26,8 +26,12 @@ void NavEKF2_core::SelectFlowFusion()
{ {
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs // If so, don't fuse measurements on this time step to reduce frame over-runs
if (magFusePerformed && dtIMUavg < 0.005f) { // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
optFlowFusionDelayed = true;
return; return;
} else {
optFlowFusionDelayed = false;
} }
// start performance timer // start performance timer

6
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -107,8 +107,12 @@ void NavEKF2_core::SelectVelPosFusion()
{ {
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs // If so, don't fuse measurements on this time step to reduce frame over-runs
if (magFusePerformed && dtIMUavg < 0.005f) { // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
posVelFusionDelayed = true;
return; return;
} else {
posVelFusionDelayed = false;
} }
// check for and read new GPS data // check for and read new GPS data

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -192,6 +192,10 @@ void NavEKF2_core::InitialiseVariables()
imuNoiseFiltState0 = 0.0f; imuNoiseFiltState0 = 0.0f;
imuNoiseFiltState1 = 0.0f; imuNoiseFiltState1 = 0.0f;
lastImuSwitchState = IMUSWITCH_MIXED; lastImuSwitchState = IMUSWITCH_MIXED;
posVelFusionDelayed = false;
optFlowFusionDelayed = false;
airSpdFusionDelayed = false;
sideSlipFusionDelayed = false;
} }
// Initialise the states from accelerometer and magnetometer data (if present) // Initialise the states from accelerometer and magnetometer data (if present)

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -759,6 +759,10 @@ private:
bool consistentMagData; // true when the magnetometers are passing consistency checks bool consistentMagData; // true when the magnetometers are passing consistency checks
bool motorsArmed; // true when the motors have been armed bool motorsArmed; // true when the motors have been armed
bool prevMotorsArmed; // value of motorsArmed from previous frame bool prevMotorsArmed; // value of motorsArmed from previous frame
bool posVelFusionDelayed; // true when the position and velocity fusion has been delayed
bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
// variables used to calulate a vertical velocity that is kinematically consistent with the verical position // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD. float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.