Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 09:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

NavEKF: IMUSwitchState enum

Browse Source
This commit is contained in:
Randy Mackay 2015-08-19 12:19:47 +09:00
parent 32c7fe996a
commit 0ea47ea7b8
2 changed files with 30 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1130,9 +1130,9 @@ void NavEKF::UpdateStrapdownEquationsNED()
// use weighted average of both IMU units for delta velocities // use weighted average of both IMU units for delta velocities
// Over-ride accelerometer blend weighting using a hard switch based on the IMU consistency and vibration monitoring checks // Over-ride accelerometer blend weighting using a hard switch based on the IMU consistency and vibration monitoring checks
if (lastImuSwitchState == 1) { if (lastImuSwitchState == IMUSWITCH_IMU0) {
IMU1_weighting = 1.0f; IMU1_weighting = 1.0f;
} else if (lastImuSwitchState == 2) { } else if (lastImuSwitchState == IMUSWITCH_IMU1) {
IMU1_weighting = 0.0f; IMU1_weighting = 0.0f;
} }
correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting); correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting);
@ -4150,29 +4150,29 @@ void NavEKF::readIMUData()
// Check the difference for excessive error and use the IMU with less noise // Check the difference for excessive error and use the IMU with less noise
// Apply hysteresis to prevent rapid switching // Apply hysteresis to prevent rapid switching
if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != 0)) { if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != IMUSWITCH_MIXED)) {
if (lastImuSwitchState == 0) { if (lastImuSwitchState == IMUSWITCH_MIXED) {
// no previous fail so switch to the IMU with least noise // no previous fail so switch to the IMU with least noise
if (imuNoiseFiltState1 < imuNoiseFiltState2) { if (imuNoiseFiltState1 < imuNoiseFiltState2) {
lastImuSwitchState = 1; lastImuSwitchState = IMUSWITCH_IMU0;
} else { } else {
lastImuSwitchState = 2; lastImuSwitchState = IMUSWITCH_IMU1;
} }
} else if (lastImuSwitchState == 1) { } else if (lastImuSwitchState == IMUSWITCH_IMU0) {
// IMU1 previously failed so require 5 m/s/s less noise on IMU2 to switch across // IMU1 previously failed so require 5 m/s/s less noise on IMU2 to switch across
if (imuNoiseFiltState1 - imuNoiseFiltState2 > 5.0f) { if (imuNoiseFiltState1 - imuNoiseFiltState2 > 5.0f) {
// IMU2 is significantly less noisy, so switch // IMU2 is significantly less noisy, so switch
lastImuSwitchState = 2; lastImuSwitchState = IMUSWITCH_IMU1;
} }
} else { } else {
// IMU2 previously failed so require 5 m/s/s less noise on IMU1 to switch across // IMU2 previously failed so require 5 m/s/s less noise on IMU1 to switch across
if (imuNoiseFiltState2 - imuNoiseFiltState1 > 5.0f) { if (imuNoiseFiltState2 - imuNoiseFiltState1 > 5.0f) {
// IMU1 is significantly less noisy, so switch // IMU1 is significantly less noisy, so switch
lastImuSwitchState = 1; lastImuSwitchState = IMUSWITCH_IMU0;
} }
} }
} else { } else {
lastImuSwitchState = 0; lastImuSwitchState = IMUSWITCH_MIXED;
} }
} else { } else {
@ -4181,16 +4181,23 @@ void NavEKF::readIMUData()
// set the switch state based on the IMU we are using to make the data source selection visible // set the switch state based on the IMU we are using to make the data source selection visible
if (ins.use_accel(0)) { if (ins.use_accel(0)) {
readDeltaVelocity(0, dVelIMU1, dtDelVel1); readDeltaVelocity(0, dVelIMU1, dtDelVel1);
lastImuSwitchState = 1; lastImuSwitchState = IMUSWITCH_IMU0;
} else if (ins.use_accel(1)) { } else if (ins.use_accel(1)) {
readDeltaVelocity(1, dVelIMU1, dtDelVel1); readDeltaVelocity(1, dVelIMU1, dtDelVel1);
lastImuSwitchState = 2; lastImuSwitchState = IMUSWITCH_IMU1;
} else { } else {
readDeltaVelocity(ins.get_primary_accel(), dVelIMU1, dtDelVel1); readDeltaVelocity(ins.get_primary_accel(), dVelIMU1, dtDelVel1);
if (ins.get_primary_accel() < 2) { switch (ins.get_primary_accel()) {
lastImuSwitchState = ins.get_primary_accel() + 1; case 0:
} else { lastImuSwitchState = IMUSWITCH_IMU0;
lastImuSwitchState = 0; break;
case 1:
lastImuSwitchState = IMUSWITCH_IMU1;
break;
default:
// we must be using IMU2 which can't be properly represented so we set to "mixed"
lastImuSwitchState = IMUSWITCH_MIXED;
break;
} }
} }
dtDelVel2 = dtDelVel1; dtDelVel2 = dtDelVel1;
@ -4770,7 +4777,7 @@ void NavEKF::InitialiseVariables()
yawResetAngleWaiting = false; yawResetAngleWaiting = false;
imuNoiseFiltState1 = 0.0f; imuNoiseFiltState1 = 0.0f;
imuNoiseFiltState2 = 0.0f; imuNoiseFiltState2 = 0.0f;
lastImuSwitchState = 0; lastImuSwitchState = IMUSWITCH_MIXED;
} }
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor

7
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -773,7 +773,12 @@ private:
float imuNoiseFiltState1; // peak hold noise estimate for IMU 1 float imuNoiseFiltState1; // peak hold noise estimate for IMU 1
float imuNoiseFiltState2; // peak hold noise estimate for IMU 2 float imuNoiseFiltState2; // peak hold noise estimate for IMU 2
Vector3f accelDiffFilt; // filtered difference between IMU 1 and 2 Vector3f accelDiffFilt; // filtered difference between IMU 1 and 2
uint8_t lastImuSwitchState; // last switch state, 0=normal, 1 = use IMU1, 2 = use IMU2 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 held by optical flow fusion across time steps // states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps // optical flow X,Y motion compensated rate measurements are fused across two time steps