mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
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:
parent
4352129c4d
commit
b36f539c7c
@ -595,6 +595,15 @@ float Aircraft::rangefinder_range() const
|
|||||||
return INFINITY;
|
return INFINITY;
|
||||||
}
|
}
|
||||||
altitude /= v.z;
|
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
|
// Add some noise on reading
|
||||||
@ -1058,6 +1067,9 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
|
|||||||
|
|
||||||
{
|
{
|
||||||
const float range = rangefinder_range();
|
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++) {
|
for (uint8_t i=0; i<ARRAY_SIZE(rangefinder_m); i++) {
|
||||||
rangefinder_m[i] = range;
|
rangefinder_m[i] = range;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user