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

AP_NavEKF: handle case where one IMU fails to return a delta velocity or angle

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-30 10:25:19 +11:00 committed by Randy Mackay
parent 69d4bd2481
commit 64ad7d6a50
1 changed files with 9 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4159,19 +4159,13 @@ void NavEKF::readIMUData()
// the imu sample time is used as a common time reference throughout the filter // the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = hal.scheduler->millis(); imuSampleTime_ms = hal.scheduler->millis();
if (ins.use_accel(0) && ins.use_accel(1)) { // dual accel mode - require both IMU's to be able to provide delta velocity outputs
// dual accel mode if (ins.use_accel(0) && ins.use_accel(1) && readDeltaVelocity(0, dVelIMU1, dtDelVel1) && readDeltaVelocity(1, dVelIMU2, dtDelVel2)) {
// read IMU1 delta velocity data
readDeltaVelocity(0, dVelIMU1, dtDelVel1);
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU1 // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU1
float alpha = 1.0f - 5.0f*dtDelVel1; float alpha = 1.0f - 5.0f*dtDelVel1;
imuNoiseFiltState1 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState1); imuNoiseFiltState1 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState1);
// read IMU2 delta velocity data
readDeltaVelocity(1, dVelIMU2, dtDelVel2);
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU2 // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU2
alpha = 1.0f - 5.0f*dtDelVel2; alpha = 1.0f - 5.0f*dtDelVel2;
imuNoiseFiltState2 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState2); imuNoiseFiltState2 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState2);
@ -4213,11 +4207,9 @@ void NavEKF::readIMUData()
// single accel mode - one of the first two accelerometers are unhealthy, not available or de-selected by the user // single accel mode - one of the first two accelerometers are unhealthy, not available or de-selected by the user
// read good accelerometer into dVelIMU1 and copy to dVelIMU2 // read good accelerometer into dVelIMU1 and copy to dVelIMU2
// 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 = IMUSWITCH_IMU0; lastImuSwitchState = IMUSWITCH_IMU0;
} else if (ins.use_accel(1)) { } else if (ins.use_accel(1) && readDeltaVelocity(1, dVelIMU1, dtDelVel1)) {
readDeltaVelocity(1, dVelIMU1, dtDelVel1);
lastImuSwitchState = IMUSWITCH_IMU1; lastImuSwitchState = IMUSWITCH_IMU1;
} else { } else {
readDeltaVelocity(ins.get_primary_accel(), dVelIMU1, dtDelVel1); readDeltaVelocity(ins.get_primary_accel(), dVelIMU1, dtDelVel1);
@ -4238,14 +4230,11 @@ void NavEKF::readIMUData()
dVelIMU2 = dVelIMU1; dVelIMU2 = dVelIMU1;
} }
if (ins.use_gyro(0) && ins.use_gyro(1)) { // Default is to use the average of two gyros if available
// dual gyro mode - average first two gyros // This reduces rate offset due to temperature variation
Vector3f dAng; Vector3f dAng0,dAng1;
dAngIMU.zero(); if (ins.use_gyro(0) && ins.use_gyro(1) && readDeltaAngle(0, dAng0) && readDeltaAngle(1, dAng1)) {
readDeltaAngle(0, dAng); dAngIMU = (dAng0+dAng1);
dAngIMU += dAng;
readDeltaAngle(1, dAng);
dAngIMU += dAng;
dAngIMU *= 0.5f; dAngIMU *= 0.5f;
} else { } else {
// single gyro mode - one of the first two gyros are unhealthy or don't exist // single gyro mode - one of the first two gyros are unhealthy or don't exist