From b559e9d331caf2f15fc4f748702d1a3775b582af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 10 Aug 2014 22:37:07 +1000 Subject: [PATCH] SITL: better rangefinder simulation able to handle a wider range, and takes account of attitude --- libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index bc0db89d0c..06714a84c1 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -63,30 +63,25 @@ float SITL_State::_gyro_drift(void) uint16_t SITL_State::_ground_sonar(float altitude) { static float home_alt = -1; - // TODO Find the current sonar object and load these params from it - // rather than assuming XL type - float scaler = 3.0f; - int16_t max_distance_cm = 700; - int16_t min_distance_cm = 20; if (home_alt == -1 && altitude > 0) home_alt = altitude; altitude = altitude - home_alt; + // adjust for apparent altitude with roll + altitude *= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg)); + altitude += _sitl->sonar_noise * _rand_float(); - if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) - altitude = max_distance_cm / 100.0f; - - altitude = constrain_float(altitude, - min_distance_cm / 100.0f, - max_distance_cm / 100.0f); - // Altitude in in m, scaler in meters/volt - float voltage = altitude / scaler; + float voltage = altitude / _sitl->sonar_scale; voltage = constrain_float(voltage, 0, 5.0f); + if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) { + voltage = 5.0f; + } + return 1023*(voltage / 5.0f); }