From 4acf4538c772fbe20c6dfd86a3c02ed7dced4410 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir@retdtechfrance.com>
Date: Wed, 1 Mar 2017 16:54:44 +0100
Subject: [PATCH] AP_HAL_SITL: rangefinder don't adjust value from FDM on
 external data

---
 libraries/AP_HAL_SITL/sitl_rangefinder.cpp | 26 +++++++++++++---------
 1 file changed, 15 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp
index e5e7c1817b..feab814488 100644
--- a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp
+++ b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp
@@ -19,14 +19,13 @@ extern const AP_HAL::HAL& hal;
 
 using namespace HALSITL;
 
-// TODO Improve that to not use analog
 /*
   setup the rangefinder with new input
  */
 void SITL_State::_update_rangefinder(float 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;
     }
 
@@ -44,24 +43,29 @@ void SITL_State::_update_rangefinder(float range_value)
         altitude -= relPosSensorEF.z;
     }
 
-    float voltage = 5.0f;
-    if (fabs(_sitl->state.rollDeg) < 90 &&
-            fabs(_sitl->state.pitchDeg) < 90) {
-        // adjust for apparent altitude with roll
-        altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg));
-
+    float voltage = 5.0f;  // Start the reading at max value = 5V
+    // 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_value, -1.0f)) {
+        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 in in m, scaler in meters/volt
         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;
         }
     }
 
-    sonar_pin_value = 1023*(voltage / 5.0f);
+    sonar_pin_value = 1023 * (voltage / 5.0f);
 }
 
 #endif