diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 5e107b4e30..306751e617 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -178,8 +178,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { AP_GROUPEND }; -AP_Proximity::AP_Proximity(AP_SerialManager &_serial_manager) : - serial_manager(_serial_manager) +AP_Proximity::AP_Proximity() { AP_Param::setup_object_defaults(this, var_info); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -284,16 +283,16 @@ void AP_Proximity::detect_instance(uint8_t instance) { uint8_t type = _type[instance]; if (type == Proximity_Type_SF40C) { - if (AP_Proximity_LightWareSF40C::detect(serial_manager)) { + if (AP_Proximity_LightWareSF40C::detect()) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], serial_manager); + drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance]); return; } } if (type == Proximity_Type_RPLidarA2) { - if (AP_Proximity_RPLidarA2::detect(serial_manager)) { + if (AP_Proximity_RPLidarA2::detect()) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], serial_manager); + drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance]); return; } } @@ -303,16 +302,16 @@ void AP_Proximity::detect_instance(uint8_t instance) return; } if (type == Proximity_Type_TRTOWER) { - if (AP_Proximity_TeraRangerTower::detect(serial_manager)) { + if (AP_Proximity_TeraRangerTower::detect()) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], serial_manager); + drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance]); return; } } if (type == Proximity_Type_TRTOWEREVO) { - if (AP_Proximity_TeraRangerTowerEvo::detect(serial_manager)) { + if (AP_Proximity_TeraRangerTowerEvo::detect()) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], serial_manager); + drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance]); return; } } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 5055ccddb3..2ead768cfe 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -18,7 +18,6 @@ #include #include #include -#include #include #define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform @@ -33,7 +32,7 @@ class AP_Proximity public: friend class AP_Proximity_Backend; - AP_Proximity(AP_SerialManager &_serial_manager); + AP_Proximity(); AP_Proximity(const AP_Proximity &other) = delete; AP_Proximity &operator=(const AP_Proximity) = delete; @@ -149,7 +148,6 @@ private: const RangeFinder *_rangefinder; uint8_t primary_instance; uint8_t num_instances; - AP_SerialManager &serial_manager; // parameters for all instances AP_Int8 _type[PROXIMITY_MAX_INSTANCES]; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 9fc1e3cf64..32c4e2af95 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -27,10 +27,10 @@ extern const AP_HAL::HAL& hal; already know that we should setup the proximity sensor */ AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state, - AP_SerialManager &serial_manager) : + AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state) { + const AP_SerialManager &serial_manager = AP::serialmanager(); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); if (uart != nullptr) { uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0)); @@ -38,9 +38,9 @@ AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend } // detect if a Lightware proximity sensor is connected by looking for a configured serial port -bool AP_Proximity_LightWareSF40C::detect(AP_SerialManager &serial_manager) +bool AP_Proximity_LightWareSF40C::detect() { - return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr; + return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr; } // update the state of the sensor diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h index 562960a486..ce1a076435 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h @@ -10,10 +10,11 @@ class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend public: // constructor - AP_Proximity_LightWareSF40C(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager); + AP_Proximity_LightWareSF40C(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state); // static detection function - static bool detect(AP_SerialManager &serial_manager); + static bool detect(); // update state void update(void) override; diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index a8ebaac088..171ad9685c 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -69,11 +69,12 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the proximity sensor */ -AP_Proximity_RPLidarA2::AP_Proximity_RPLidarA2(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state, - AP_SerialManager &serial_manager) : - AP_Proximity_Backend(_frontend, _state) +AP_Proximity_RPLidarA2::AP_Proximity_RPLidarA2( + AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) : + AP_Proximity_Backend(_frontend, _state) { + const AP_SerialManager &serial_manager = AP::serialmanager(); _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); if (_uart != nullptr) { _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0)); @@ -84,9 +85,9 @@ AP_Proximity_RPLidarA2::AP_Proximity_RPLidarA2(AP_Proximity &_frontend, } // detect if a RPLidarA2 proximity sensor is connected by looking for a configured serial port -bool AP_Proximity_RPLidarA2::detect(AP_SerialManager &serial_manager) +bool AP_Proximity_RPLidarA2::detect() { - return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr; + return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr; } // update the _rp_state of the sensor diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h index d75936e381..26a2648701 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h @@ -39,10 +39,10 @@ class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend public: // constructor - AP_Proximity_RPLidarA2(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager); + AP_Proximity_RPLidarA2(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); // static detection function - static bool detect(AP_SerialManager &serial_manager); + static bool detect(); // update state void update(void) override; diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index a8b31faf55..4b1dc9e806 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -27,11 +27,13 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the proximity sensor */ -AP_Proximity_TeraRangerTower::AP_Proximity_TeraRangerTower(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state, - AP_SerialManager &serial_manager) : +AP_Proximity_TeraRangerTower::AP_Proximity_TeraRangerTower( + AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state) { + const AP_SerialManager &serial_manager = AP::serialmanager(); + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); if (uart != nullptr) { uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0)); @@ -39,10 +41,10 @@ AP_Proximity_TeraRangerTower::AP_Proximity_TeraRangerTower(AP_Proximity &_fronte } // detect if a TeraRanger Tower proximity sensor is connected by looking for a configured serial port -bool AP_Proximity_TeraRangerTower::detect(AP_SerialManager &serial_manager) +bool AP_Proximity_TeraRangerTower::detect() { AP_HAL::UARTDriver *uart = nullptr; - uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); + uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); return uart != nullptr; } diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h index 880a849850..5449382fa9 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h @@ -10,10 +10,10 @@ class AP_Proximity_TeraRangerTower : public AP_Proximity_Backend public: // constructor - AP_Proximity_TeraRangerTower(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager); + AP_Proximity_TeraRangerTower(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); // static detection function - static bool detect(AP_SerialManager &serial_manager); + static bool detect(); // update state void update(void) override; diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp index 0da1d247a6..5672f7bacc 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp @@ -28,10 +28,11 @@ extern const AP_HAL::HAL& hal; already know that we should setup the proximity sensor */ AP_Proximity_TeraRangerTowerEvo::AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state, - AP_SerialManager &serial_manager) : + AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state) { + const AP_SerialManager &serial_manager = AP::serialmanager(); + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); if (uart != nullptr) { uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0)); @@ -40,11 +41,9 @@ AP_Proximity_TeraRangerTowerEvo::AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_ } // detect if a TeraRanger Tower proximity sensor is connected by looking for a configured serial port -bool AP_Proximity_TeraRangerTowerEvo::detect(AP_SerialManager &serial_manager) +bool AP_Proximity_TeraRangerTowerEvo::detect() { - AP_HAL::UARTDriver *uart = nullptr; - uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); - return uart != nullptr; + return (AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr); } // update the state of the sensor diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h index e886542500..e2dafc797c 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h @@ -9,10 +9,10 @@ class AP_Proximity_TeraRangerTowerEvo : public AP_Proximity_Backend { public: // constructor - AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager); + AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); // static detection function - static bool detect(AP_SerialManager &serial_manager); + static bool detect(); // update state void update(void) override;