From 98a06c566b145c5bf73e1cb3dff665cbc602c871 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 3 Jan 2025 12:14:39 +1100
Subject: [PATCH] AP_HAL_SITL: create structures to hold serial rangefinder
 init info

---
 libraries/AP_HAL_SITL/SITL_State_common.cpp | 23 +++++++++++++++++++--
 libraries/AP_HAL_SITL/SITL_State_common.h   |  3 +++
 2 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp
index 47a625d4d7..0e765c556f 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp
@@ -19,14 +19,30 @@
 #include <AP_Param/AP_Param.h>
 #include <SITL/SIM_JSBSim.h>
 #include <AP_HAL/utility/Socket_native.h>
-
-extern const AP_HAL::HAL& hal;
+#include "SITL_State_common.h"
 
 using namespace HALSITL;
 
+static const struct {
+    const char *name;
+    SITL::SerialRangeFinder *(*createfn)();
+}  serial_rangefinder_definitions[] {
+};
+
 #define streq(a, b) (!strcmp(a, b))
 SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const char *arg, const uint8_t portNumber)
 {
+    for (const auto &definition : serial_rangefinder_definitions) {
+        if (!streq(definition.name, name)) {
+            continue;
+        }
+        if (num_serial_rangefinders >= ARRAY_SIZE(serial_rangefinders)) {
+            AP_HAL::panic("Too many simulated serial rangefinders");
+        }
+        serial_rangefinders[num_serial_rangefinders] = definition.createfn();
+        return serial_rangefinders[num_serial_rangefinders++];
+    }
+
     if (streq(name, "benewake_tf02")) {
         if (benewake_tf02 != nullptr) {
             AP_HAL::panic("Only one benewake_tf02 at a time");
@@ -408,6 +424,9 @@ void SITL_State_Common::sim_update(void)
     if (gyus42v2 != nullptr) {
         gyus42v2->update(sitl_model->rangefinder_range());
     }
+    for (uint8_t i=0; i<num_serial_rangefinders; i++) {
+        serial_rangefinders[i]->update(sitl_model->rangefinder_range());
+    }
     if (efi_ms != nullptr) {
         efi_ms->update();
     }
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h
index 472f7dfefa..3155036a60 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.h
+++ b/libraries/AP_HAL_SITL/SITL_State_common.h
@@ -125,6 +125,9 @@ public:
     SITL::Vicon *vicon;
 #endif
 
+    SITL::SerialRangeFinder *serial_rangefinders[16];
+    uint8_t num_serial_rangefinders;
+
     // simulated Ainstein LR-D1 rangefinder:
     SITL::RF_Ainstein_LR_D1 *ainsteinlrd1;
     // simulated Benewake tf02 rangefinder: