diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 991b1fb76d..8d5b42c988 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 44df7b5e6d..3ce2de4c49 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/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); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 136229cc13..6dd10b7261 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index 5c445fa489..0b2ddd2c79 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/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); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 38b90da988..361f1f3028 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index 6b7d14c86d..ed1ce832ba 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/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); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index beb0a811f8..e0cd9bd3bc 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/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; } /* diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index c1053e86de..dddc65d210 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/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); diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 292e4843fc..c9122d6f0b 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/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