Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Use nominated IMU unless unavailable 

We now run a separate instance of the EKF for each IMU, so input data fusion of IMU's is no longer required.
This commit is contained in:
Paul Riseborough 2015-11-05 13:05:11 +11:00 committed by Andrew Tridgell
parent 392fce76b2
commit d48454ee2d
3 changed files with 10 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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

3
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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;

11
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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;