diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 1b867102be..4ee78408a2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -281,101 +281,19 @@ void NavEKF2_core::readIMUData() // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = hal.scheduler->millis(); - if (ins.use_accel(0) && ins.use_accel(1)) { - // dual accel mode - // delta time from each IMU - float dtDelVel0 = dtIMUavg; - float dtDelVel1 = dtIMUavg; - // delta velocity vector from each IMU - Vector3f delVel0, delVel1; - - // Get delta velocity and time data from each IMU - readDeltaVelocity(0, delVel0, dtDelVel0); - readDeltaVelocity(1, delVel1, dtDelVel1); - - // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 0 - float alpha = 1.0f - 5.0f*dtDelVel0; - imuNoiseFiltState0 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState0); - - // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 1 - alpha = 1.0f - 5.0f*dtDelVel1; - imuNoiseFiltState1 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState1); - - // calculate the filtered difference between acceleration vectors from IMU 0 and 1 - // apply a LPF filter with a 1.0 second time constant - alpha = constrain_float(0.5f*(dtDelVel0 + dtDelVel1),0.0f,1.0f); - accelDiffFilt = (ins.get_accel(0) - ins.get_accel(1)) * alpha + accelDiffFilt * (1.0f - alpha); - float accelDiffLength = accelDiffFilt.length(); - - // Check the difference for excessive error and use the IMU with less noise - // Apply hysteresis to prevent rapid switching - if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != IMUSWITCH_MIXED)) { - if (lastImuSwitchState == IMUSWITCH_MIXED) { - // no previous fail so switch to the IMU with least noise - if (imuNoiseFiltState0 < imuNoiseFiltState1) { - lastImuSwitchState = IMUSWITCH_IMU0; - // Get data from IMU 0 - imuDataNew.delVel = delVel0; - imuDataNew.delVelDT = dtDelVel0; - } else { - lastImuSwitchState = IMUSWITCH_IMU1; - // Get data from IMU 1 - imuDataNew.delVel = delVel1; - imuDataNew.delVelDT = dtDelVel1; - } - } else if (lastImuSwitchState == IMUSWITCH_IMU0) { - // IMU 1 previously failed so require 5 m/s/s less noise on IMU 1 to switch - if (imuNoiseFiltState0 - imuNoiseFiltState1 > 5.0f) { - // IMU 1 is significantly less noisy, so switch - lastImuSwitchState = IMUSWITCH_IMU1; - // Get data from IMU 1 - imuDataNew.delVel = delVel1; - imuDataNew.delVelDT = dtDelVel1; - } - } else { - // IMU 0 previously failed so require 5 m/s/s less noise on IMU 0 to switch across - if (imuNoiseFiltState1 - imuNoiseFiltState0 > 5.0f) { - // IMU 0 is significantly less noisy, so switch - lastImuSwitchState = IMUSWITCH_IMU0; - // Get data from IMU 0 - imuDataNew.delVel = delVel0; - imuDataNew.delVelDT = dtDelVel0; - } - } - } else { - lastImuSwitchState = IMUSWITCH_MIXED; - // Use a blend of both accelerometers - imuDataNew.delVel = (delVel0 + delVel1)*0.5f; - imuDataNew.delVelDT = (dtDelVel0 + dtDelVel1)*0.5f; - } + // use the nominated imu or primary if not available + if (ins.use_accel(imu_index)) { + readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); } else { - // single accel mode - one of the first two accelerometers are unhealthy, not available or de-selected by the user - // set the switch state based on the IMU we are using to make the data source selection visible - if (ins.use_accel(0)) { - readDeltaVelocity(0, imuDataNew.delVel, imuDataNew.delVelDT); - lastImuSwitchState = IMUSWITCH_IMU0; - } else if (ins.use_accel(1)) { - readDeltaVelocity(1, imuDataNew.delVel, imuDataNew.delVelDT); - lastImuSwitchState = IMUSWITCH_IMU1; - } else { - readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); - switch (ins.get_primary_accel()) { - case 0: - lastImuSwitchState = IMUSWITCH_IMU0; - break; - case 1: - lastImuSwitchState = IMUSWITCH_IMU1; - break; - default: - // we must be using an IMU which can't be properly represented so we set to "mixed" - lastImuSwitchState = IMUSWITCH_MIXED; - break; - } - } + readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); } - // Get delta angle data from promary gyro - readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); + // Get delta angle data from primary gyro or primary if not available + if (ins.use_gyro(imu_index)) { + readDeltaAngle(imu_index, imuDataNew.delAng); + } else { + readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); + } imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f); // get current time stamp diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 453d81196a..c21f26daa5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -191,9 +191,6 @@ void NavEKF2_core::InitialiseVariables() memset(&statesArray, 0, sizeof(statesArray)); posDownDerivative = 0.0f; posDown = 0.0f; - imuNoiseFiltState0 = 0.0f; - imuNoiseFiltState1 = 0.0f; - lastImuSwitchState = IMUSWITCH_MIXED; posVelFusionDelayed = false; optFlowFusionDelayed = false; airSpdFusionDelayed = false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 36ffa1e2de..2741117dd3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -803,17 +803,6 @@ private: uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight. - // monitoring IMU quality - float imuNoiseFiltState0; // peak hold noise estimate for IMU 0 - float imuNoiseFiltState1; // peak hold noise estimate for IMU 1 - Vector3f accelDiffFilt; // filtered difference between IMU 0 and 1 - enum ImuSwitchState { - IMUSWITCH_MIXED=0, // IMU 0 & 1 are mixed - IMUSWITCH_IMU0, // only IMU 0 is used - IMUSWITCH_IMU1 // only IMU 1 is used - }; - ImuSwitchState lastImuSwitchState; // last switch state (see imuSwitchState enum) - // States used for unwrapping of compass yaw error float innovationIncrement; float lastInnovation;