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:
parent
3ee88112cf
commit
78ac1340c8
@ -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)));
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user