From a1eb284349cb9f5d2563f332058e78e1cf754efe Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 29 Jul 2020 09:26:36 +0100 Subject: [PATCH] AP_HAL_SITL: populate first rangefinder distance if unused --- libraries/AP_HAL_SITL/sitl_rangefinder.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp index 31df356340..da84122e4f 100644 --- a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp +++ b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp @@ -65,6 +65,11 @@ void SITL_State::_update_rangefinder(float range_value) } } + // if not populated also fill out the STIL rangefinder array + if (is_equal(_sitl->state.rangefinder_m[0],-1.0f)) { + _sitl->state.rangefinder_m[0] = altitude; + } + sonar_pin_value = 1023 * (voltage / 5.0f); }