Skip to content

Commit

Permalink
SITL: avoid floating point exception around rangefinder distance
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
peterbarker committed Oct 10, 2024
1 parent 4352129 commit b36f539
Showing 1 changed file with 12 additions and 0 deletions.
12 changes: 12 additions & 0 deletions libraries/SITL/SIM_Aircraft.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit b36f539

Please sign in to comment.