AP_NavEKF: Prevent false triggering of optical flow takeoff detection

Now that we have a pre-arm check in place to detect bad lidar, the motion check is unnecessary and can false trigger for copters with flexible undercarriages or on uneven ground.
This commit is contained in:
Paul Riseborough 2015-08-11 03:57:16 +10:00 committed by Randy Mackay
parent 3ee88112cf
commit 78ac1340c8

View File

@ -5288,18 +5288,7 @@ void NavEKF::readRangeFinder(void)
void NavEKF::detectOptFlowTakeoff(void)
{
if (vehicleArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
const AP_InertialSensor &ins = _ahrs->get_ins();
Vector3f angRateVec;
Vector3f gyroBias;
getGyroBias(gyroBias);
bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1);
if (dual_ins) {
angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias;
} else {
angRateVec = ins.get_gyro() - gyroBias;
}
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rangeAtArming + 0.1f)));
takeOffDetected = (takeOffDetected || (rngMea > (rangeAtArming + 0.1f)));
}
}