Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Peter Barker 2024-10-10 12:16:55 +11:00 committed by Peter Barker
parent 4352129c4d
commit b36f539c7c
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/SITL/SIM_Aircraft.cpp
View File

@ -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<ARRAY_SIZE(rangefinder_m); i++) {
rangefinder_m[i] = range;
}