mirror of https://github.com/ArduPilot/ardupilot
NavEKF: IMU ratio set to primary accel when neither IMU is used
This forces the EKF IMU ratio to indicate which IMU is used except that it will be "0" in the unlikely case that the third IMU is used
This commit is contained in:
parent
559cc29635
commit
c179ed5253
|
@ -4187,7 +4187,11 @@ void NavEKF::readIMUData()
|
|||
lastImuSwitchState = 2;
|
||||
} else {
|
||||
readDeltaVelocity(ins.get_primary_accel(), dVelIMU1, dtDelVel1);
|
||||
lastImuSwitchState = 0;
|
||||
if (ins.get_primary_accel() < 2) {
|
||||
lastImuSwitchState = ins.get_primary_accel() + 1;
|
||||
} else {
|
||||
lastImuSwitchState = 0;
|
||||
}
|
||||
}
|
||||
dtDelVel2 = dtDelVel1;
|
||||
dVelIMU2 = dVelIMU1;
|
||||
|
|
Loading…
Reference in New Issue