From b36f539c7c9db63514882fcf81bac0f00d58574f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Oct 2024 12:16:55 +1100 Subject: [PATCH] SITL: avoid floating point exception around rangefinder distance projecting onto an infinite plane can cause exceptionally long rangefinder distances - for now jsut cap the distance that the simulated rangefinder can return to avoid floating point exceptions. the FPE is caused in the Plane FlyEachFrame autotest when flying quadplane-copter_tailsitter - which ends up with a rangefinder at yaw-minus-180. --- libraries/SITL/SIM_Aircraft.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 22825b95ca..c0469abc7c 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -595,6 +595,15 @@ float Aircraft::rangefinder_range() const return INFINITY; } altitude /= v.z; + + // this is awful, but there are drawbacks to assuming an + // infinite plane. If we don't do this here then we end up + // with a ridiculous rangefinder range, and that can cause + // floating point exceptions when we return a distance in cm + // from the AP_RangeFinder_SITL. + if (altitude > 100000) { + return INFINITY; + } } // Add some noise on reading @@ -1058,6 +1067,9 @@ void Aircraft::update_external_payload(const struct sitl_input &input) { const float range = rangefinder_range(); + if (!isinf(range) && range > 100000) { + AP_HAL::panic("Bad rangefinder calculation"); + } for (uint8_t i=0; i