AP_HAL_SITL: consolidate rangefinder range calculations

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -74,7 +74,6 @@ void SITL_State::_sitl_setup(const char *home_str)
     if (_sitl != nullptr) {
         // setup some initial values
         _update_airspeed(0);
-        _update_rangefinder(0);
         if (enable_gimbal) {
             gimbal = new SITL::Gimbal(_sitl->state);
         }
@@ -165,7 +164,7 @@ void SITL_State::_fdm_input_step(void)
 
     if (_sitl != nullptr) {
         _update_airspeed(_sitl->state.airspeed);
-        _update_rangefinder(_sitl->state.range);
+        _update_rangefinder();
     }
 
     // trigger all APM timers.

libraries/AP_HAL_SITL/SITL_State.h

@@ -139,7 +139,7 @@ private:
     void _setup_adc(void);
 
     void set_height_agl(void);
-    void _update_rangefinder(float range_value);
+    void _update_rangefinder();
     void _set_signal_handlers(void) const;
 
     void _update_airspeed(float airspeed);
@@ -279,6 +279,11 @@ private:
 
     // simulated GPS devices
     SITL::GPS *gps[2];  // constrained by # of parameter sets
+
+    // returns a voltage between 0V to 5V which should appear as the
+    // voltage from the sensor
+    float _sonar_pin_voltage() const;
+
 };
 
 #endif // defined(HAL_BUILD_AP_PERIPH)

libraries/AP_HAL_SITL/sitl_rangefinder.cpp

@@ -19,58 +19,36 @@ extern const AP_HAL::HAL& hal;
 
 using namespace HALSITL;
 
+// returns a voltage between 0V to 5V which should appear as the
+// voltage from the sensor
+float SITL_State::_sonar_pin_voltage() const
+{
+    // 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) {
+        // glitched
+        return 5.0f;
+    }
+
+    const float altitude = sitl_model->rangefinder_range();
+    if (altitude == INFINITY) {
+        return 5.0f;
+    }
+
+    // Altitude in in m, scaler in meters/volt
+    const float voltage = altitude / _sitl->sonar_scale;
+
+    // constrain to 0-5V
+    return constrain_float(voltage, 0.0f, 5.0f);
+}
+
 /*
   setup the rangefinder with new input
  */
-void SITL_State::_update_rangefinder(float range_value)
+void SITL_State::_update_rangefinder()
 {
-    float altitude = range_value;
-    if (is_equal(range_value, -1.0f)) {  // Use SITL altitude as reading by default
-        altitude = _sitl->height_agl;
-    }
-
-    // sensor position offset in body frame
-    const Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset;
-
-    // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
-    if (!relPosSensorBF.is_zero()) {
-        // get a rotation matrix following DCM conventions (body to earth)
-        Matrix3f rotmat;
-        _sitl->state.quaternion.rotation_matrix(rotmat);
-        // rotate the offset into earth frame
-        const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
-        // correct the altitude at the sensor
-        altitude -= relPosSensorEF.z;
-    }
-
-    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;
-        // constrain to 0-5V
-        voltage = constrain_float(voltage, 0.0f, 5.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;
-        }
-    }
-
-    // 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);
+    sonar_pin_value = 1023 * (_sonar_pin_voltage() / 5.0f);
 }
 
 #endif