From e2bb0c1608930ed8f28d787a71924cd933ee4fbe Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 15 Apr 2026 12:24:56 +0200 Subject: [PATCH] - Revert "Check lat,lon null using float instead of double" This reverts commit 2b9d24fb32df136d32e4907e96ef61d6cdf5f678. - smaller null island threshold 1e-7 deg = 1.1 cm 1e-12 deg = 0.1 um, while still easily catching the exact 0 from missing init - add comment explaining checks --- src/modules/navigator/rtl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e4f0eb2a27..817ba40149 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -53,6 +53,7 @@ using matrix::wrap_pi; static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We don't consider safe points valid if the distance from the current home to the safe point is smaller than this distance static constexpr float MIN_DIST_THRESHOLD = 2.f; +static constexpr double NULL_ISLAND_THRESHOLD_DEG{1e-12}; RTL::RTL(Navigator *navigator) : NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), @@ -585,9 +586,9 @@ bool RTL::extractValidSafePointPosition(const mission_item_s &safe_point_item, f position.lon = safe_point_item.lon; position.yaw = NAN; + // basic checks protect against invalid data in case of missing init or cache read errors return PX4_ISFINITE(position.lat) && PX4_ISFINITE(position.lon) && PX4_ISFINITE(position.alt) - && !((fabsf(static_cast(position.lat)) < FLT_EPSILON) - && (fabsf(static_cast(position.lon)) < FLT_EPSILON)) + && !((fabs(position.lat) < NULL_ISLAND_THRESHOLD_DEG) && (fabs(position.lon) < NULL_ISLAND_THRESHOLD_DEG)) && (fabs(position.lat) <= 90.0) && (fabs(position.lon) <= 180.0); }