AP_Rangefinder: support multiple serial rangefinders

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

@@ -25,12 +25,13 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
-                                                   AP_SerialManager &serial_manager) :
+                                                   AP_SerialManager &serial_manager,
+                                                   uint8_t serial_instance) :
     AP_RangeFinder_Backend(_state)
 {
-    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
     if (uart != nullptr) {
-        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
+        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, serial_instance));
     }
 }
 
@@ -39,9 +40,9 @@ AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_Stat
    trying to take a reading on Serial. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_LeddarOne::detect(AP_SerialManager &serial_manager)
+bool AP_RangeFinder_LeddarOne::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
 {
-    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr;
+    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
 }
 
 // read - return last value measured by sensor

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h

@@ -43,10 +43,11 @@ class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
 public:
     // constructor
     AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
-                             AP_SerialManager &serial_manager);
+                             AP_SerialManager &serial_manager,
+                             uint8_t serial_instance);
 
     // static detection function
-    static bool detect(AP_SerialManager &serial_manager);
+    static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
 
     // update state
     void update(void);

libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp

@@ -26,12 +26,13 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
-                                                               AP_SerialManager &serial_manager) :
+                                                               AP_SerialManager &serial_manager,
+                                                               uint8_t serial_instance) :
     AP_RangeFinder_Backend(_state)
 {
-    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
     if (uart != nullptr) {
-        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
+        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, serial_instance));
     }
 }
 
@@ -40,9 +41,9 @@ AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::Rang
    trying to take a reading on Serial. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_LightWareSerial::detect(AP_SerialManager &serial_manager)
+bool AP_RangeFinder_LightWareSerial::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
 {
-    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr;
+    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
 }
 
 // read - return last value measured by sensor

libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h

@@ -9,10 +9,11 @@ class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend
 public:
     // constructor
     AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
-                                   AP_SerialManager &serial_manager);
+                                   AP_SerialManager &serial_manager,
+                                   uint8_t serial_instance);
 
     // static detection function
-    static bool detect(AP_SerialManager &serial_manager);
+    static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
 
     // update state
     void update(void);

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp

@@ -30,12 +30,13 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state,
-                                                                 AP_SerialManager &serial_manager) :
+                                                                 AP_SerialManager &serial_manager,
+                                                                 uint8_t serial_instance) :
     AP_RangeFinder_Backend(_state)
 {
-    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
     if (uart != nullptr) {
-        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
+        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, serial_instance));
     }
 }
 
@@ -44,9 +45,9 @@ AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::Ra
    trying to take a reading on Serial. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_MaxsonarSerialLV::detect(AP_SerialManager &serial_manager)
+bool AP_RangeFinder_MaxsonarSerialLV::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
 {
-    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr;
+    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
 }
 
 // read - return last value measured by sensor

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h

@@ -9,10 +9,11 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend
 public:
     // constructor
     AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state,
-                                   AP_SerialManager &serial_manager);
+                                   AP_SerialManager &serial_manager,
+                                   uint8_t serial_instance);
 
     // static detection function
-    static bool detect(AP_SerialManager &serial_manager);
+    static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
 
     // update state
     void update(void);

libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp

@@ -33,10 +33,11 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
-                                                 AP_SerialManager &serial_manager) :
+                                                 AP_SerialManager &serial_manager,
+                                                 uint8_t serial_instance) :
     AP_RangeFinder_Backend(_state)
 {
-    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
     if (uart != nullptr) {
         uart->begin(ULANDING_BAUD, ULANDING_BUFSIZE_RX, ULANDING_BUFSIZE_TX);
     }
@@ -47,9 +48,9 @@ AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State
    trying to take a reading on Serial. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_uLanding::detect(AP_SerialManager &serial_manager)
+bool AP_RangeFinder_uLanding::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
 {
-    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr;
+    return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
 }
 
 /*

libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h

@@ -9,10 +9,11 @@ class AP_RangeFinder_uLanding : public AP_RangeFinder_Backend
 public:
     // constructor
 	AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
-                            AP_SerialManager &serial_manager);
+                            AP_SerialManager &serial_manager,
+                            uint8_t serial_instance);
 
     // static detection function
-    static bool detect(AP_SerialManager &serial_manager);
+    static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
 
     // update state
     void update(void);

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -553,8 +553,10 @@ void RangeFinder::init(void)
         // init called a 2nd time?
         return;
     }
-    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
-        detect_instance(i);
+    for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+        // serial_instance will be increased inside detect_instance
+        // if a serial driver is loaded for this instance
+        detect_instance(i, serial_instance);
         if (drivers[i] != nullptr) {
             // we loaded a driver for this instance, so it must be
             // present (although it may not be healthy)
@@ -607,7 +609,7 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
 /*
   detect if an instance of a rangefinder is connected. 
  */
-void RangeFinder::detect_instance(uint8_t instance)
+void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
 {
     enum RangeFinder_Type _type = (enum RangeFinder_Type)state[instance].type.get();
     switch (_type) {
@@ -671,21 +673,21 @@ void RangeFinder::detect_instance(uint8_t instance)
         break;
 #endif
     case RangeFinder_TYPE_LWSER:
-        if (AP_RangeFinder_LightWareSerial::detect(serial_manager)) {
+        if (AP_RangeFinder_LightWareSerial::detect(serial_manager, serial_instance)) {
             state[instance].instance = instance;
-            drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], serial_manager);
+            drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], serial_manager, serial_instance++);
         }
         break;
     case RangeFinder_TYPE_LEDDARONE:
-        if (AP_RangeFinder_LeddarOne::detect(serial_manager)) {
+        if (AP_RangeFinder_LeddarOne::detect(serial_manager, serial_instance)) {
             state[instance].instance = instance;
-            drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], serial_manager);
+            drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], serial_manager, serial_instance++);
         }
         break;
     case RangeFinder_TYPE_ULANDING:
-        if (AP_RangeFinder_uLanding::detect(serial_manager)) {
+        if (AP_RangeFinder_uLanding::detect(serial_manager, serial_instance)) {
             state[instance].instance = instance;
-            drivers[instance] = new AP_RangeFinder_uLanding(state[instance], serial_manager);
+            drivers[instance] = new AP_RangeFinder_uLanding(state[instance], serial_manager, serial_instance++);
         }
         break;
 #if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
@@ -704,9 +706,9 @@ void RangeFinder::detect_instance(uint8_t instance)
         }
         break;
     case RangeFinder_TYPE_MBSER:
-        if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_manager)) {
+        if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_manager, serial_instance)) {
             state[instance].instance = instance;
-            drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], serial_manager);
+            drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], serial_manager, serial_instance++);
         }
         break;
     case RangeFinder_TYPE_ANALOG:

libraries/AP_RangeFinder/RangeFinder.h

@@ -167,7 +167,7 @@ private:
     AP_SerialManager &serial_manager;
     Vector3f pos_offset_zero;   // allows returning position offsets of zero for invalid requests
 
-    void detect_instance(uint8_t instance);
+    void detect_instance(uint8_t instance, uint8_t& serial_instance);
     void update_instance(uint8_t instance);  
 
     bool _add_backend(AP_RangeFinder_Backend *driver);