Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: tidy range calculations

Browse Source 
Also return INFINITY if the rangefinder can't see the ground
This commit is contained in:
Peter Barker 2021-12-08 12:49:54 +11:00 committed by Peter Barker
parent acbe567202
commit 6fcf724b67
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_Aircraft.cpp
View File

@ -507,16 +507,17 @@ float Aircraft::rangefinder_range() const
altitude -= relPosSensorEF.z;
}
// If the attidude is non reversed for SITL OR we are using rangefinder from external simulator,
// We adjust the reading with noise, glitch and scaler as the reading is on analog port.
if ((fabs(sitl->state.rollDeg) < 90.0 && fabs(sitl->state.pitchDeg) < 90.0) || !is_equal(range, -1.0f)) {
if (is_equal(range, -1.0f)) { // disable for external reading that already handle this
// adjust for apparent altitude with roll
altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg));
if (is_equal(range, -1.0f)) { // disable for external reading that already handle this
if (fabs(sitl->state.rollDeg) >= 90.0 || fabs(sitl->state.pitchDeg) >= 90.0) {
// not going to hit the ground....
return INFINITY;
}
// Add some noise on reading
altitude += sitl->sonar_noise * rand_float();
// adjust for apparent altitude with roll
altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg));
}
// Add some noise on reading
altitude += sitl->sonar_noise * rand_float();
return altitude;
}