From 348f0ad590aa66b946056fcf365028fdb97895fa Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 18 Apr 2022 15:30:09 +1000
Subject: [PATCH] SITL: add SIM_SONAR_ROT, use it for measuring horizontal
 distances

---
 libraries/SITL/SIM_Aircraft.cpp              |  11 +-
 libraries/SITL/SIM_SerialProximitySensor.cpp |  94 -----------------
 libraries/SITL/SIM_SerialProximitySensor.h   |  10 +-
 libraries/SITL/SITL.cpp                      | 104 ++++++++++++++++++-
 libraries/SITL/SITL.h                        |   3 +
 5 files changed, 118 insertions(+), 104 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 62d23d659ef..63e16e6b4f8 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -483,11 +483,19 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
 // is bouncing off:
 float Aircraft::perpendicular_distance_to_rangefinder_surface() const
 {
-    return sitl->height_agl;
+    switch ((Rotation)sitl->sonar_rot.get()) {
+    case Rotation::ROTATION_PITCH_270:
+        return sitl->height_agl;
+    case ROTATION_NONE ... ROTATION_YAW_315:
+        return sitl->measure_distance_at_angle_bf(location, sitl->sonar_rot.get()*45);
+    default:
+        AP_BoardConfig::config_error("Bad simulated sonar rotation");
+    }
 }
 
 float Aircraft::rangefinder_range() const
 {
+
     float roll = sitl->state.rollDeg;
     float pitch = sitl->state.pitchDeg;
 
@@ -524,6 +532,7 @@ float Aircraft::rangefinder_range() const
     // sensor position offset in body frame
     const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;
 
+    // n.b. the following code is assuming rotation-pitch-270:
     // 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)
diff --git a/libraries/SITL/SIM_SerialProximitySensor.cpp b/libraries/SITL/SIM_SerialProximitySensor.cpp
index aa2c0575016..b357551ebdd 100644
--- a/libraries/SITL/SIM_SerialProximitySensor.cpp
+++ b/libraries/SITL/SIM_SerialProximitySensor.cpp
@@ -43,98 +43,4 @@ void SerialProximitySensor::update(const Location &location)
     write_to_autopilot((char*)data, packetlen);
 }
 
-float SerialProximitySensor::measure_distance_at_angle_bf(const Location &location, float angle) const
-{
-    const SIM *sitl = AP::sitl();
-
-    Vector2f vehicle_pos_cm;
-    if (!location.get_vector_xy_from_origin_NE(vehicle_pos_cm)) {
-        // should probably use SITL variables...
-        return 0.0f;
-    }
-    static uint64_t count = 0;
-
-    if (count == 0) {
-        unlink("/tmp/rayfile.scr");
-        unlink("/tmp/intersectionsfile.scr");
-    }
-
-    count++;
-
-    // the 1000 here is so the files don't grow unbounded
-    const bool write_debug_files = count < 1000;
-
-    FILE *rayfile = nullptr;
-    if (write_debug_files) {
-        rayfile = fopen("/tmp/rayfile.scr", "a");
-    }
-    // cast a ray from location out 200m...
-    Location location2 = location;
-    location2.offset_bearing(wrap_180(angle + sitl->state.yawDeg), 200);
-    Vector2f ray_endpos_cm;
-    if (!location2.get_vector_xy_from_origin_NE(ray_endpos_cm)) {
-        // should probably use SITL variables...
-        return 0.0f;
-    }
-    if (rayfile != nullptr) {
-        ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7);
-        fclose(rayfile);
-    }
-
-    // setup a grid of posts
-    FILE *postfile = nullptr;
-    FILE *intersectionsfile = nullptr;
-    if (write_debug_files) {
-        static bool postfile_written;
-        if (!postfile_written) {
-            ::fprintf(stderr, "Writing /tmp/post-locations.scr\n");
-            postfile_written = true;
-            postfile = fopen("/tmp/post-locations.scr", "w");
-        }
-        intersectionsfile = fopen("/tmp/intersections.scr", "a");
-    }
-    const float radius_cm = 100.0f;
-    float min_dist_cm = 1000000.0;
-    const uint8_t num_post_offset = 10;
-    for (int8_t x=-num_post_offset; x<num_post_offset; x++) {
-        for (int8_t y=-num_post_offset; y<num_post_offset; y++) {
-            Location post_location = post_origin;
-            post_location.offset(x*10+3, y*10+2);
-            if (postfile != nullptr) {
-                ::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm/100.0);
-            }
-            Vector2f post_position_cm;
-            if (!post_location.get_vector_xy_from_origin_NE(post_position_cm)) {
-                // should probably use SITL variables...
-                return 0.0f;
-            }
-            Vector2f intersection_point_cm;
-            if (Vector2f::circle_segment_intersection(ray_endpos_cm, vehicle_pos_cm, post_position_cm, radius_cm, intersection_point_cm)) {
-                float dist_cm = (intersection_point_cm-vehicle_pos_cm).length();
-                if (intersectionsfile != nullptr) {
-                    Location intersection_point = location;
-                    intersection_point.offset(intersection_point_cm.x/100.0,
-                                              intersection_point_cm.y/100.0);
-                    ::fprintf(intersectionsfile,
-                              "map icon %f %f barrell\n",
-                              intersection_point.lat*1e-7,
-                              intersection_point.lng*1e-7);
-                }
-                if (dist_cm < min_dist_cm) {
-                    min_dist_cm = dist_cm;
-                }
-            }
-        }
-    }
-    if (postfile != nullptr) {
-        fclose(postfile);
-    }
-    if (intersectionsfile != nullptr) {
-        fclose(intersectionsfile);
-    }
-
-    // ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm/100.0f);
-    return min_dist_cm / 100.0f;
-}
-
 #endif  // HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED
diff --git a/libraries/SITL/SIM_SerialProximitySensor.h b/libraries/SITL/SIM_SerialProximitySensor.h
index a8baad3a4ac..db94bef9a92 100644
--- a/libraries/SITL/SIM_SerialProximitySensor.h
+++ b/libraries/SITL/SIM_SerialProximitySensor.h
@@ -49,18 +49,14 @@ public:
                                          uint8_t buflen) = 0;
 
     // return distance to nearest object at angle
-    float measure_distance_at_angle_bf(const Location &location, float angle) const;
+    float measure_distance_at_angle_bf(const Location &location, float angle) const {
+        return AP::sitl()->measure_distance_at_angle_bf(location, angle);
+    }
 
 private:
 
     uint32_t last_sent_ms;
 
-    const Location post_origin {
-        518752066,
-        146487830,
-        0,
-        Location::AltFrame::ABSOLUTE
-    };
 };
 
 }
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 8007e4e4e1f..7e07c8efb6f 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -54,6 +54,7 @@ const AP_Param::GroupInfo SIM::var_info[] = {
     AP_GROUPINFO("WIND_DIR",      10, SIM,  wind_direction,  180),
     AP_GROUPINFO("WIND_TURB",     11, SIM,  wind_turbulance,  0),
     AP_GROUPINFO("SERVO_SPEED",   16, SIM,  servo_speed,  0.14),
+    AP_GROUPINFO("SONAR_ROT",     17, SIM,  sonar_rot, Rotation::ROTATION_PITCH_270),
     AP_GROUPINFO("BATT_VOLTAGE",  19, SIM,  batt_voltage,  12.6f),
     AP_GROUPINFO("BATT_CAP_AH",   20, SIM,  batt_capacity_ah,  0),
     AP_GROUPINFO("SONAR_GLITCH",  23, SIM,  sonar_glitch, 0),
@@ -508,7 +509,14 @@ const AP_Param::GroupInfo SIM::var_ins[] = {
 #endif  // HAL_INS_TEMPERATURE_CAL_ENABLE
     AP_GROUPEND
 };
-    
+
+const Location post_origin {
+    518752066,
+    146487830,
+    0,
+    Location::AltFrame::ABSOLUTE
+};
+
 /* report SITL state via MAVLink SIMSTATE*/
 void SIM::simstate_send(mavlink_channel_t chan) const
 {
@@ -649,8 +657,100 @@ float SIM::get_rangefinder(uint8_t instance) {
     return -1;
 };
 
-} // namespace SITL
+float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const
+{
+    // should we populate state.rangefinder_m[...] from this?
+    Vector2f vehicle_pos_cm;
+    if (!location.get_vector_xy_from_origin_NE(vehicle_pos_cm)) {
+        // should probably use SITL variables...
+        return 0.0f;
+    }
+    static uint64_t count = 0;
 
+    if (count == 0) {
+        unlink("/tmp/rayfile.scr");
+        unlink("/tmp/intersectionsfile.scr");
+    }
+
+    count++;
+
+    // the 1000 here is so the files don't grow unbounded
+    const bool write_debug_files = count < 1000;
+
+    FILE *rayfile = nullptr;
+    if (write_debug_files) {
+        rayfile = fopen("/tmp/rayfile.scr", "a");
+    }
+    // cast a ray from location out 200m...
+    Location location2 = location;
+    location2.offset_bearing(wrap_180(angle + state.yawDeg), 200);
+    Vector2f ray_endpos_cm;
+    if (!location2.get_vector_xy_from_origin_NE(ray_endpos_cm)) {
+        // should probably use SITL variables...
+        return 0.0f;
+    }
+    if (rayfile != nullptr) {
+        ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7);
+        fclose(rayfile);
+    }
+
+    // setup a grid of posts
+    FILE *postfile = nullptr;
+    FILE *intersectionsfile = nullptr;
+    if (write_debug_files) {
+        static bool postfile_written;
+        if (!postfile_written) {
+            ::fprintf(stderr, "Writing /tmp/post-locations.scr\n");
+            postfile_written = true;
+            postfile = fopen("/tmp/post-locations.scr", "w");
+        }
+        intersectionsfile = fopen("/tmp/intersections.scr", "a");
+    }
+    const float radius_cm = 100.0f;
+    float min_dist_cm = 1000000.0;
+    const uint8_t num_post_offset = 10;
+    for (int8_t x=-num_post_offset; x<num_post_offset; x++) {
+        for (int8_t y=-num_post_offset; y<num_post_offset; y++) {
+            Location post_location = post_origin;
+            post_location.offset(x*10+3, y*10+2);
+            if (postfile != nullptr) {
+                ::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm/100.0);
+            }
+            Vector2f post_position_cm;
+            if (!post_location.get_vector_xy_from_origin_NE(post_position_cm)) {
+                // should probably use SITL variables...
+                return 0.0f;
+            }
+            Vector2f intersection_point_cm;
+            if (Vector2f::circle_segment_intersection(ray_endpos_cm, vehicle_pos_cm, post_position_cm, radius_cm, intersection_point_cm)) {
+                float dist_cm = (intersection_point_cm-vehicle_pos_cm).length();
+                if (intersectionsfile != nullptr) {
+                    Location intersection_point = location;
+                    intersection_point.offset(intersection_point_cm.x/100.0,
+                                              intersection_point_cm.y/100.0);
+                    ::fprintf(intersectionsfile,
+                              "map icon %f %f barrell\n",
+                              intersection_point.lat*1e-7,
+                              intersection_point.lng*1e-7);
+                }
+                if (dist_cm < min_dist_cm) {
+                    min_dist_cm = dist_cm;
+                }
+            }
+        }
+    }
+    if (postfile != nullptr) {
+        fclose(postfile);
+    }
+    if (intersectionsfile != nullptr) {
+        fclose(intersectionsfile);
+    }
+
+    // ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm/100.0f);
+    return min_dist_cm / 100.0f;
+}
+
+} // namespace SITL
 
 namespace AP {
 
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index b3e95e0b87a..e34d9f15f94 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -181,6 +181,7 @@ public:
     AP_Float sonar_glitch;// probability between 0-1 that any given sonar sample will read as max distance
     AP_Float sonar_noise; // in metres
     AP_Float sonar_scale; // meters per volt
+    AP_Int8 sonar_rot;  // from rotations enumeration
 
     AP_Float drift_speed; // degrees/second/minute
     AP_Float drift_time;  // period in minutes
@@ -447,6 +448,8 @@ public:
     // get the rangefinder reading for the desired instance, returns -1 for no data
     float get_rangefinder(uint8_t instance);
 
+    float measure_distance_at_angle_bf(const Location &location, float angle) const;
+
     // get the apparent wind speed and direction as set by external physics backend
     float get_apparent_wind_dir() const{return state.wind_vane_apparent.direction;}
     float get_apparent_wind_spd() const{return state.wind_vane_apparent.speed;}