diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index e6876b07a4484..bb43be98839ec 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -383,6 +383,17 @@ void NavEKF3_core::detectFlight() } if (!onGround) { +#if APM_BUILD_TYPE(APM_BUILD_ArduSub) + // If depth has increased since arming, then we definitely are diving + if ((stateStruct.position.z - posDownAtTakeoff) > 1.5f) { + inFlight = true; + } + + // If rangefinder has decreased since arming, then we definitely are diving + if ((rangeDataNew.rng - rngAtStartOfFlight) < -0.5f) { + inFlight = true; + } +#else // If height has increased since exiting on-ground, then we definitely are flying if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) { inFlight = true; @@ -392,6 +403,7 @@ void NavEKF3_core::detectFlight() if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) { inFlight = true; } +#endif // If more than 5 seconds since likely_flying was set // true, then set inFlight true