From 7d232b24d64cc823e0707fbdbf5c6fc12411a2cb Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 12 Aug 2020 12:58:15 +1000
Subject: [PATCH] SITL: improve simulated serial proximity sensor

SITL: rename measure_distance_at_angle to include '_bf'
---
 libraries/SITL/SIM_PS_RPLidarA2.cpp          |  2 +-
 libraries/SITL/SIM_SerialProximitySensor.cpp | 37 +++++++++++++-------
 libraries/SITL/SIM_SerialProximitySensor.h   |  2 +-
 3 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/libraries/SITL/SIM_PS_RPLidarA2.cpp b/libraries/SITL/SIM_PS_RPLidarA2.cpp
index d304997c56..33bdd6bf13 100644
--- a/libraries/SITL/SIM_PS_RPLidarA2.cpp
+++ b/libraries/SITL/SIM_PS_RPLidarA2.cpp
@@ -161,7 +161,7 @@ void PS_RPLidarA2::update_output_scan(const Location &location)
 
 
         const float MAX_RANGE = 16.0f;
-        float distance = measure_distance_at_angle(location, current_degrees_bf);
+        float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
         // ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
         if (distance > MAX_RANGE) {
             // sensor returns zero for out-of-range
diff --git a/libraries/SITL/SIM_SerialProximitySensor.cpp b/libraries/SITL/SIM_SerialProximitySensor.cpp
index 44be3f99b0..7e4b13cfc0 100644
--- a/libraries/SITL/SIM_SerialProximitySensor.cpp
+++ b/libraries/SITL/SIM_SerialProximitySensor.cpp
@@ -41,16 +41,22 @@ void SerialProximitySensor::update(const Location &location)
     write_to_autopilot((char*)data, packetlen);
 }
 
-float SerialProximitySensor::measure_distance_at_angle(const Location &location, float angle) const
+float SerialProximitySensor::measure_distance_at_angle_bf(const Location &location, float angle) const
 {
-    // const SITL *sitl = AP::sitl();
+    const SITL *sitl = AP::sitl();
 
-    Vector2f pos1_cm;
-    if (!location.get_vector_xy_from_origin_NE(pos1_cm)) {
+    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
@@ -62,9 +68,9 @@ float SerialProximitySensor::measure_distance_at_angle(const Location &location,
     }
     // cast a ray from location out 200m...
     Location location2 = location;
-    location2.offset_bearing(wrap_180(angle), 200);
-    Vector2f pos2_cm;
-    if (!location2.get_vector_xy_from_origin_NE(pos2_cm)) {
+    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;
     }
@@ -77,27 +83,32 @@ float SerialProximitySensor::measure_distance_at_angle(const Location &location,
     FILE *postfile = nullptr;
     FILE *intersectionsfile = nullptr;
     if (write_debug_files) {
-        postfile = fopen("/tmp/post-locations.scr", "w");
+        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+0.5, y*10+0.5);
+            post_location.offset(x*10+3, y*10+2);
             if (postfile != nullptr) {
-                ::fprintf(postfile, "map icon %f %f hoop\n", post_location.lat*1e-7, post_location.lng*1e-7);
+                ::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;
             }
-            const float radius_cm = 10.0f;
             Vector2f intersection_point_cm;
-            if (Vector2f::circle_segment_intersection(pos1_cm, pos2_cm, post_position_cm, radius_cm, intersection_point_cm)) {
-                float dist_cm = (intersection_point_cm-pos1_cm).length();
+            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.0f,
diff --git a/libraries/SITL/SIM_SerialProximitySensor.h b/libraries/SITL/SIM_SerialProximitySensor.h
index 998bd1de94..b25c38829b 100644
--- a/libraries/SITL/SIM_SerialProximitySensor.h
+++ b/libraries/SITL/SIM_SerialProximitySensor.h
@@ -41,7 +41,7 @@ public:
                                          uint8_t buflen) = 0;
 
     // return distance to nearest object at angle
-    float measure_distance_at_angle(const Location &location, float angle) const;
+    float measure_distance_at_angle_bf(const Location &location, float angle) const;
 
 private: