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

AP_HAL_SITL: rangefinder don't adjust value from FDM on external data

This commit is contained in:
Pierre Kancir 2017-03-01 16:54:44 +01:00 committed by Andrew Tridgell
parent 599e3d7b83
commit 4acf4538c7
1 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_HAL_SITL/sitl_rangefinder.cpp
View File

@ -19,14 +19,13 @@ extern const AP_HAL::HAL& hal;
using namespace HALSITL; using namespace HALSITL;
// TODO Improve that to not use analog
/* /*
setup the rangefinder with new input setup the rangefinder with new input
*/ */
void SITL_State::_update_rangefinder(float range_value) void SITL_State::_update_rangefinder(float range_value)
{ {
float altitude = range_value; float altitude = range_value;
if (range_value == -1) { if (is_equal(range_value, -1.0f)) { // Use SITL altitude as reading by default
altitude = _sitl->height_agl; altitude = _sitl->height_agl;
} }
@ -44,24 +43,29 @@ void SITL_State::_update_rangefinder(float range_value)
altitude -= relPosSensorEF.z; altitude -= relPosSensorEF.z;
} }
float voltage = 5.0f; float voltage = 5.0f; // Start the reading at max value = 5V
if (fabs(_sitl->state.rollDeg) < 90 && // If the attidude is non reversed for SITL OR we are using rangefinder from external simulator,
fabs(_sitl->state.pitchDeg) < 90) { // We adjust the reading with noise, glitch and scaler as the reading is on analog port.
// adjust for apparent altitude with roll if ((fabs(_sitl->state.rollDeg) < 90.0 && fabs(_sitl->state.pitchDeg) < 90.0) || !is_equal(range_value, -1.0f)) {
altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg)); if (is_equal(range_value, -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));
}
// Add some noise on reading
altitude += _sitl->sonar_noise * rand_float(); altitude += _sitl->sonar_noise * rand_float();
// Altitude in in m, scaler in meters/volt // Altitude in in m, scaler in meters/volt
voltage = altitude / _sitl->sonar_scale; voltage = altitude / _sitl->sonar_scale;
voltage = constrain_float(voltage, 0, 5.0f); // constrain to 0-5V
voltage = constrain_float(voltage, 0.0f, 5.0f);
if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) { // Use glitch defines as the probablility between 0-1 that any given sonar sample will read as max distance
if (!is_zero(_sitl->sonar_glitch) && _sitl->sonar_glitch >= (rand_float() + 1.0f) / 2.0f) {
voltage = 5.0f; voltage = 5.0f;
} }
} }
sonar_pin_value = 1023*(voltage / 5.0f); sonar_pin_value = 1023 * (voltage / 5.0f);
} }
#endif #endif