From 965cdd7e72174bbdc29d958185631bbe58b2025e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 12 Jul 2022 14:29:35 +0900
Subject: [PATCH] AP_Proximity: fixup detection of serial drivers

---
 libraries/AP_Proximity/AP_Proximity.cpp       | 167 +++++++++---------
 libraries/AP_Proximity/AP_Proximity.h         |   2 -
 .../AP_Proximity_Backend_Serial.cpp           |  16 +-
 .../AP_Proximity_Backend_Serial.h             |   7 +-
 .../AP_Proximity_LightWareSF45B.h             |   5 +-
 5 files changed, 98 insertions(+), 99 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index 391aa30cb8..52f2d5ad45 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -109,16 +109,92 @@ void AP_Proximity::init()
         // init called a 2nd time?
         return;
     }
-    for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) {
-        detect_instance(i);
-        if (drivers[i] != nullptr) {
+
+    // instantiate backends
+    uint8_t serial_instance = 0;
+    for (uint8_t instance=0; instance<PROXIMITY_MAX_INSTANCES; instance++) {
+        switch (get_type(instance)) {
+        case Type::None:
+            break;
+        case Type::RPLidarA2:
+            if (AP_Proximity_RPLidarA2::detect(serial_instance)) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance], serial_instance);
+                serial_instance++;
+            }
+            break;
+        case Type::MAV:
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
+            break;
+
+        case Type::TRTOWER:
+            if (AP_Proximity_TeraRangerTower::detect(serial_instance)) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance], serial_instance);
+                serial_instance++;
+            }
+            break;
+        case Type::TRTOWEREVO:
+            if (AP_Proximity_TeraRangerTowerEvo::detect(serial_instance)) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance], serial_instance);
+                serial_instance++;
+            }
+            break;
+
+        case Type::RangeFinder:
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
+            break;
+
+        case Type::SF40C:
+            if (AP_Proximity_LightWareSF40C::detect(serial_instance)) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance], serial_instance);
+                serial_instance++;
+            }
+            break;
+
+        case Type::SF45B:
+            if (AP_Proximity_LightWareSF45B::detect(serial_instance)) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance], serial_instance);
+                serial_instance++;
+            }
+            break;
+
+        case Type::CYGBOT_D1:
+#if AP_PROXIMITY_CYGBOT_ENABLED
+        if (AP_Proximity_Cygbot_D1::detect(serial_instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance], serial_instance);
+            serial_instance++;
+        }
+# endif
+        break;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        case Type::SITL:
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
+            break;
+
+        case Type::AirSimSITL:
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
+            break;
+#endif
+        } // switch (get_type(instance))
+
+        if (drivers[instance] != nullptr) {
             // we loaded a driver for this instance, so it must be
             // present (although it may not be healthy)
-            num_instances = i+1;
+            num_instances = instance+1;
         }
 
         // initialise status
-        state[i].status = Status::NotConnected;
+        state[instance].status = Status::NotConnected;
     }
 }
 
@@ -383,87 +459,6 @@ bool AP_Proximity::valid_instance(uint8_t i) const
     return (Type)params[i].type.get() != Type::None;
 }
 
-//  detect if an instance of a proximity sensor is connected.
-void AP_Proximity::detect_instance(uint8_t instance)
-{
-    switch (get_type(instance)) {
-    case Type::None:
-        return;
-    case Type::RPLidarA2:
-        if (AP_Proximity_RPLidarA2::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-    case Type::MAV:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
-        return;
-
-    case Type::TRTOWER:
-        if (AP_Proximity_TeraRangerTower::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-    case Type::TRTOWEREVO:
-        if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-
-    case Type::RangeFinder:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
-        return;
-
-    case Type::SF40C:
-        if (AP_Proximity_LightWareSF40C::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-
-    case Type::SF45B:
-        if (AP_Proximity_LightWareSF45B::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-
-    case Type::CYGBOT_D1:
-#if AP_PROXIMITY_CYGBOT_ENABLED
-    if (AP_Proximity_Cygbot_D1::detect(instance)) {
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]);
-        return;
-    }
-# endif
-    break;
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    case Type::SITL:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
-        return;
-
-    case Type::AirSimSITL:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
-        return;
-
-#endif
-    }
-}
-
-
-
 AP_Proximity *AP_Proximity::_singleton;
 
 namespace AP {
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index c08f0672d2..9dd2948574 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -170,8 +170,6 @@ private:
     // return true if the given instance exists
     bool valid_instance(uint8_t i) const;
 
-    void detect_instance(uint8_t instance);
-
     // parameters for all instances
     AP_Int8 _raw_log_enable;                            // enable logging raw distances
     AP_Int8 _ign_gnd_enable;                           // true if land detection should be enabled
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp
index 7bb9bb2462..8d0de98c7d 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp
@@ -25,22 +25,24 @@
 */
 AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend,
                                                          AP_Proximity::Proximity_State &_state,
-                                                         AP_Proximity_Params &_params) :
+                                                         AP_Proximity_Params &_params,
+                                                         uint8_t serial_instance) :
     AP_Proximity_Backend(_frontend, _state, _params)
 {
     const AP_SerialManager &serial_manager = AP::serialmanager();
-    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
+    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, serial_instance);
     if (_uart != nullptr) {
         // start uart with larger receive buffer
-        _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0), rxspace(), 0);
+        _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, serial_instance), rxspace(), 0);
     }
 }
 
-// detect if a proximity sensor is connected by looking for a
-// configured serial port
-bool AP_Proximity_Backend_Serial::detect(uint8_t instance)
+// static detection function
+// detect if a proximity sensor is connected by looking for a configured serial port
+// serial_instance affects which serial port is used.  Should be 0 or 1 depending on whether this is the 1st or 2nd proximity sensor with a serial interface
+bool AP_Proximity_Backend_Serial::detect(uint8_t serial_instance)
 {
-    return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, instance);
+    return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, serial_instance);
 }
 
 #endif // HAL_PROXIMITY_ENABLED
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h
index e0164d6239..ac363fbe46 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h
+++ b/libraries/AP_Proximity/AP_Proximity_Backend_Serial.h
@@ -9,10 +9,13 @@ class AP_Proximity_Backend_Serial : public AP_Proximity_Backend
 public:
     AP_Proximity_Backend_Serial(AP_Proximity &_frontend,
                                 AP_Proximity::Proximity_State &_state,
-                                AP_Proximity_Params& _params);
+                                AP_Proximity_Params& _params,
+                                uint8_t serial_instance);
 
     // static detection function
-    static bool detect(uint8_t instance);
+    // detect if a proximity sensor is connected by looking for a configured serial port
+    // serial_instance affects which serial port is used.  Should be 0 or 1 depending on whether this is the 1st or 2nd proximity sensor with a serial interface
+    static bool detect(uint8_t serial_instance);
 
 protected:
     virtual uint16_t rxspace() const { return 0; };
diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
index 95dc0eb65b..fb32d7ca23 100644
--- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
+++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
@@ -12,8 +12,9 @@ public:
     // constructor
     AP_Proximity_LightWareSF45B(AP_Proximity &_frontend,
                                 AP_Proximity::Proximity_State &_state,
-                                AP_Proximity_Params& _params) :
-            AP_Proximity_LightWareSerial(_frontend, _state, _params) {}
+                                AP_Proximity_Params& _params,
+                                uint8_t serial_instance) :
+            AP_Proximity_LightWareSerial(_frontend, _state, _params, serial_instance) {}
 
     uint16_t rxspace() const override {
         return 1280;