SITL: make range finder a standalone sitl plugin

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -90,6 +90,7 @@ void SITL_State::_sitl_setup(const char *home_str)
         _update_ins(0);
         _update_compass();
         _update_gps(0, 0, 0, 0, 0, 0, false);
+        _update_rangefinder(0);
 #endif
         if (enable_gimbal) {
             gimbal = new SITL::Gimbal(_sitl->state);
@@ -167,6 +168,7 @@ void SITL_State::_fdm_input_step(void)
                     !_sitl->gps_disable);
         _update_ins(_sitl->state.airspeed);
         _update_compass();
+        _update_rangefinder(_sitl->state.range);
 
         if (_sitl->adsb_plane_count >= 0 &&
             adsb == nullptr) {

libraries/AP_HAL_SITL/SITL_State.h

@@ -84,7 +84,7 @@ private:
 
     void set_height_agl(void);
     void _update_compass(void);
-
+    void _update_rangefinder(float range_value);
     void _set_signal_handlers(void) const;
 
     struct gps_data {

libraries/AP_HAL_SITL/sitl_ins.cpp

@@ -78,49 +78,6 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed)
     return airspeed_raw/4;
 }
 
-
-/*
-  emulate an analog rangefinder
- */
-uint16_t SITL_State::_ground_sonar(void)
-{
-    float altitude = _sitl->height_agl;
-
-    // sensor position offset in body frame
-    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
-        Vector3f relPosSensorEF = rotmat * relPosSensorBF;
-        // correct the altitude at the sensor
-        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));
-
-        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);
-
-        if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) {
-            voltage = 5.0f;
-        }
-    }
-
-    return 1023*(voltage / 5.0f);
-}
-
 /*
   setup the INS input channels with new input
  */
@@ -131,7 +88,6 @@ void SITL_State::_update_ins(float airspeed)
         return;
     }
 
-    sonar_pin_value    = _ground_sonar();
     float airspeed_simulated = (fabsf(_sitl->arspd_fail) > 1.0e-6f) ? _sitl->arspd_fail : airspeed;
     airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * rand_float()));
 }

libraries/AP_HAL_SITL/sitl_rangefinder.cpp

@@ -0,0 +1,67 @@
+/*
+  SITL handling
+
+  This simulates a rangefinder
+
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "AP_HAL_SITL.h"
+#include "AP_HAL_SITL_Namespace.h"
+#include "HAL_SITL_Class.h"
+#include "SITL_State.h"
+#include <SITL/SITL.h>
+#include <AP_Math/AP_Math.h>
+
+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) {
+        altitude = _sitl->height_agl;
+    }
+
+    // sensor position offset in body frame
+    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
+        Vector3f relPosSensorEF = rotmat * relPosSensorBF;
+        // correct the altitude at the sensor
+        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));
+
+        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);
+
+        if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) {
+            voltage = 5.0f;
+        }
+    }
+
+    sonar_pin_value = 1023*(voltage / 5.0f);
+}
+
+#endif

libraries/SITL/SIM_Aircraft.cpp

@@ -161,6 +161,7 @@ void Aircraft::update_position(void)
 
     // we only advance time if it hasn't been advanced already by the
     // backend
+    // TODO : make this a function
     if (last_time_us == time_now_us) {
         time_now_us += frame_time_us;
     }
@@ -362,6 +363,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.rpm1 = rpm1;
     fdm.rpm2 = rpm2;
     fdm.rcin_chan_count = rcin_chan_count;
+    fdm.range = range;
     memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));
     fdm.bodyMagField = mag_bf;
 

libraries/SITL/SIM_Aircraft.h

@@ -139,6 +139,7 @@ protected:
     float rpm2 = 0;
     uint8_t rcin_chan_count = 0;
     float rcin[8];
+    float range = -1;          // rangefinder detection in m
 
     // Wind Turbulence simulated Data
     float turbulence_azimuth = 0;

libraries/SITL/SITL.h

@@ -25,6 +25,7 @@ struct sitl_fdm {
     double rpm2;            // secondary RPM
     uint8_t rcin_chan_count;
     float  rcin[8];         // RC input 0..1
+    double range;           // rangefinder value
     Vector3f bodyMagField;  // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
     Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes
 };