SITL: correct submarine rangefinding

libraries/SITL/SIM_Aircraft.cpp

@@ -384,7 +384,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.vtol_motor_start = vtol_motor_start;
     memcpy(fdm.rpm, rpm, num_motors * sizeof(float));
     fdm.rcin_chan_count = rcin_chan_count;
-    fdm.range = range;
+    fdm.range = rangefinder_range();
     memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));
     fdm.bodyMagField = mag_bf;
 
@@ -484,15 +484,48 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     }
 }
 
+// returns perpendicular height to surface downward-facing rangefinder
+// is bouncing off:
+float Aircraft::perpendicular_distance_to_rangefinder_surface() const
+{
+    return sitl->height_agl;
+}
+
 float Aircraft::rangefinder_range() const
 {
-    // swiped from sitl_rangefinder.cpp - we should unify them at some stage
+    float roll = sitl->state.rollDeg;
+    float pitch = sitl->state.pitchDeg;
 
-    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;
+    if (roll > 0) {
+        roll -= rangefinder_beam_width();
+        if (roll < 0) {
+            roll = 0;
+        }
+    } else {
+        roll += rangefinder_beam_width();
+        if (roll > 0) {
+            roll = 0;
+        }
+    }
+    if (pitch > 0) {
+        pitch -= rangefinder_beam_width();
+        if (pitch < 0) {
+            pitch = 0;
+        }
+    } else {
+        pitch += rangefinder_beam_width();
+        if (pitch > 0) {
+            pitch = 0;
+        }
     }
 
+    if (fabs(roll) >= 90.0 || fabs(pitch) >= 90.0) {
+        // not going to hit the ground....
+        return INFINITY;
+    }
+
+    float altitude = perpendicular_distance_to_rangefinder_surface();
+
     // sensor position offset in body frame
     const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;
 
@@ -507,15 +540,9 @@ float Aircraft::rangefinder_range() const
         altitude -= relPosSensorEF.z;
     }
 
-    if (is_equal(range, -1.0f)) {  // disable for external reading that already handle this
-        if (fabs(sitl->state.rollDeg) >= 90.0 || fabs(sitl->state.pitchDeg) >= 90.0) {
-            // not going to hit the ground....
-            return INFINITY;
-        }
-
     // adjust for apparent altitude with roll
-        altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg));
-    }
+    altitude /= cosf(radians(roll)) * cosf(radians(pitch));
+
     // Add some noise on reading
     altitude += sitl->sonar_noise * rand_float();
 

libraries/SITL/SIM_Aircraft.h

@@ -192,7 +192,9 @@ protected:
     float rpm[12];
     uint8_t rcin_chan_count;
     float rcin[12];
-    float range = -1.0f;                 // externally supplied rangefinder value, assumed to have been corrected for vehicle attitude
+
+    virtual float rangefinder_beam_width() const { return 0; }
+    virtual float perpendicular_distance_to_rangefinder_surface() const;
 
     struct {
         // data from simulated laser scanner, if available

libraries/SITL/SIM_Submarine.cpp

@@ -65,6 +65,12 @@ Submarine::Submarine(const char *frame_str) :
     lock_step_scheduled = true;
 }
 
+float Submarine::perpendicular_distance_to_rangefinder_surface() const
+{
+    const float floor_depth = calculate_sea_floor_depth(position);
+    return floor_depth - position.z;
+}
+
 // calculate rotational and linear accelerations
 void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
 {
@@ -88,9 +94,8 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
         rot_accel += t.rotational * thrust * frame_property.thruster_mount_radius / frame_property.moment_of_inertia;
     }
 
-    float floor_depth = calculate_sea_floor_depth(position);
-    range = floor_depth - position.z;
     // Limit movement at the sea floor
+    const float floor_depth = calculate_sea_floor_depth(position);
     if (position.z > floor_depth && body_accel.z > -GRAVITY_MSS) {
     	body_accel.z = -GRAVITY_MSS;
     }
@@ -138,7 +143,7 @@ void Submarine::calculate_buoyancy_torque(Vector3f &torque)
  * @param position
  * @return float
  */
-float Submarine::calculate_sea_floor_depth(const Vector3d &/*position*/)
+float Submarine::calculate_sea_floor_depth(const Vector3d &/*position*/) const
 {
     return 50;
 }

libraries/SITL/SIM_Submarine.h

@@ -89,8 +89,12 @@ protected:
 
     bool on_ground() const override;
 
+    float rangefinder_beam_width() const override { return 10; }
+
+    float perpendicular_distance_to_rangefinder_surface() const override;
+
     // calculate sea floor depth based for terrain follow
-    float calculate_sea_floor_depth(const Vector3d &/*position*/);
+    float calculate_sea_floor_depth(const Vector3d &/*position*/) const;
     // calculate rotational and linear accelerations
     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
     // calculate buoyancy