From a94ebaef76ab8a2991901b8c9156417aba0062f0 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Thu, 11 Apr 2024 09:15:51 -0700 Subject: [PATCH] Sub: set inFlight to true if sub descends or rangefinder value decreases --- libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 47e42773613ff..85e583b3f4e79 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -379,6 +379,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 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; @@ -388,6 +399,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 @@ -395,7 +407,6 @@ void NavEKF3_core::detectFlight() inFlight = true; } } - } // Store vehicle height and range prior to takeoff for use in post takeoff checks