SITL: move calculation of simulated rangefinder range to inside SIM_Aircraft

this will allow us to use the for non-serial rangefinder backends

libraries/SITL/SIM_Aircraft.cpp

@@ -431,6 +431,44 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     }
 }
 
+float Aircraft::rangefinder_range() const
+{
+    // swiped from sitl_rangefinder.cpp - we should unify them at some stage
+
+    float altitude = range;  // only sub appears to set this
+    if (is_equal(altitude, -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;
+    }
+
+    // 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, -1.0f)) {
+        if (is_equal(range, -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();
+    }
+
+    return altitude;
+}
+
+
 uint64_t Aircraft::get_wall_time_us() const
 {
 #if defined(__CYGWIN__) || defined(__CYGWIN64__)

libraries/SITL/SIM_Aircraft.h

@@ -126,7 +126,9 @@ public:
     const Location &get_location() const { return location; }
 
     const Vector3f &get_position() const { return position; }
-    const float &get_range() const { return range; }
+
+    // distance the rangefinder is perceiving
+    float rangefinder_range() const;
 
     void get_attitude(Quaternion &attitude) const {
         attitude.from_rotation_matrix(dcm);
@@ -170,7 +172,7 @@ protected:
     float rpm[12];
     uint8_t rcin_chan_count = 0;
     float rcin[12];
-    float range = -1.0f;                 // rangefinder detection in m
+    float range = -1.0f;                 // externally supplied rangefinder value, assumed to have been corrected for vehicle attitude
 
     struct {
         // data from simulated laser scanner, if available

libraries/SITL/SIM_SerialRangeFinder.cpp

@@ -20,45 +20,6 @@
 
 using namespace SITL;
 
-uint16_t SerialRangeFinder::calculate_range_cm(float range_value) const
-{
-    // swiped from sitl_rangefinder.cpp - we should unify them at some stage
-
-    const SITL *sitl = AP::sitl();
-
-    float altitude = range_value;
-    if (is_equal(range_value, -1.0f)) {  // Use SITL altitude as reading by default
-        altitude = AP::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;
-    }
-
-    // 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();
-    }
-
-    return altitude * 100.0f;
-}
-
 void SerialRangeFinder::update(float range)
 {
     // just send a chunk of data at 5Hz:
@@ -68,8 +29,9 @@ void SerialRangeFinder::update(float range)
     }
     last_sent_ms = now;
 
+    const uint16_t range_cm = uint16_t(range*100);
     uint8_t data[255];
-    const uint32_t packetlen = packet_for_alt(calculate_range_cm(range),
+    const uint32_t packetlen = packet_for_alt(range_cm,
                                               data,
                                               ARRAY_SIZE(data));
 

libraries/SITL/SIM_SerialRangeFinder.h

@@ -41,9 +41,6 @@ public:
 private:
 
     uint32_t last_sent_ms;
-
-    uint16_t calculate_range_cm(float range_value) const;
-
 };
 
 }