From 29292c2aaa6c2c4049b86a71df6815b856a57896 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 1 Jun 2023 12:47:43 +0900
Subject: [PATCH] SITL: simulated SF45b fixes and enhancements

resolve crash if time_delta_ms is too long
correct sample_count calculation
sends one reading per 3deg (closer to real device)
distance max is 53m
returns -1m on failure
sweeps back and forth -190~190 deg
---
 libraries/SITL/SIM_PS_LightWare_SF45B.cpp | 33 +++++++++++++++--------
 libraries/SITL/SIM_PS_LightWare_SF45B.h   |  3 ++-
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/libraries/SITL/SIM_PS_LightWare_SF45B.cpp b/libraries/SITL/SIM_PS_LightWare_SF45B.cpp
index f585cb1c1e..dd1f0b4c08 100644
--- a/libraries/SITL/SIM_PS_LightWare_SF45B.cpp
+++ b/libraries/SITL/SIM_PS_LightWare_SF45B.cpp
@@ -183,17 +183,17 @@ void PS_LightWare_SF45B::update_output_responses()
 
 void PS_LightWare_SF45B::update_output_scan(const Location &location)
 {
-    const uint32_t now = AP_HAL::millis();
-    if (last_scan_output_time_ms == 0) {
-        last_scan_output_time_ms = now;
+    const uint32_t now_ms = AP_HAL::millis();
+    const uint32_t time_delta_ms = (now_ms - last_scan_output_time_ms);
+    if (time_delta_ms > 1000) {
+        last_scan_output_time_ms = now_ms;
         return;
     }
-    const uint32_t time_delta = (now - last_scan_output_time_ms);
 
-    const uint32_t samples_per_second = 1000;
+    const uint32_t samples_per_second = 133;
     const float samples_per_ms = samples_per_second / 1000.0f;
-    const uint32_t sample_count = time_delta / samples_per_ms;
-    const float degrees_per_ms = 1000 / 1000.0f;  // Randy reports 1 degree increments
+    const uint32_t sample_count = samples_per_ms * time_delta_ms;
+    const float degrees_per_ms = 390 / 1000.0f;
     const float degrees_per_sample = degrees_per_ms / samples_per_ms;
 
 //    ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta, degrees_per_sample);
@@ -201,16 +201,27 @@ void PS_LightWare_SF45B::update_output_scan(const Location &location)
     last_scan_output_time_ms += sample_count/samples_per_ms;
 
     for (uint32_t i=0; i<sample_count; i++) {
-        const float current_degrees_bf = fmod((last_degrees_bf + degrees_per_sample), 360.0f);
+
+        const float ANGLE_MIN_DEG = -170;
+        const float ANGLE_MAX_DEG = +170;
+        float current_degrees_bf = last_degrees_bf + (last_dir * degrees_per_sample);
+        if (current_degrees_bf < ANGLE_MIN_DEG) {
+            current_degrees_bf += (ANGLE_MIN_DEG - current_degrees_bf);
+            last_dir = -last_dir;
+        }
+        if (current_degrees_bf > ANGLE_MAX_DEG) {
+            current_degrees_bf += (ANGLE_MAX_DEG - current_degrees_bf);
+            last_dir = -last_dir;
+        }
         last_degrees_bf = current_degrees_bf;
 
 
-        const float MAX_RANGE = 16.0f;
+        const float MAX_RANGE = 53.0f;
         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
-            distance = 0.0f;
+            // sensor returns -1 for out-of-range
+            distance = -1.0f;
         }
 
         PackedMessage<DistanceDataCM> packed_distance_data {
diff --git a/libraries/SITL/SIM_PS_LightWare_SF45B.h b/libraries/SITL/SIM_PS_LightWare_SF45B.h
index cee06d4ea6..901a6c7988 100644
--- a/libraries/SITL/SIM_PS_LightWare_SF45B.h
+++ b/libraries/SITL/SIM_PS_LightWare_SF45B.h
@@ -259,7 +259,8 @@ private:
 
     uint32_t last_scan_output_time_ms;
 
-    float last_degrees_bf;
+    float last_degrees_bf;  // previous iteration's lidar angle
+    float last_dir = 1;     // previous iterations movement direction.  +1 CW, -1 for CCW
 
 };