From 0d4e72622e0fc38d013fd67902ea8976b53e152c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Dec 2019 15:53:05 +1100 Subject: [PATCH] AP_Proximity: RPLidarA2 uses intermediate serial class --- .../AP_Proximity/AP_Proximity_RPLidarA2.cpp | 27 ------------------- .../AP_Proximity/AP_Proximity_RPLidarA2.h | 10 +++---- 2 files changed, 3 insertions(+), 34 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index b8e5f0f7c6..eac134559f 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -28,7 +28,6 @@ #include #include "AP_Proximity_RPLidarA2.h" -#include #include #include @@ -64,32 +63,6 @@ extern const AP_HAL::HAL& hal; -/* - The constructor also initialises the proximity sensor. Note that this - 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_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)); - } - _cnt = 0 ; - _sync_error = 0 ; - _byte_count = 0; -} - -// detect if a RPLidarA2 proximity sensor is connected by looking for a configured serial port -bool AP_Proximity_RPLidarA2::detect() -{ - return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr; -} - // update the _rp_state of the sensor void AP_Proximity_RPLidarA2::update(void) { diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h index 85aae9bc9e..6238ceca33 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h @@ -30,19 +30,16 @@ #pragma once #include "AP_Proximity.h" -#include "AP_Proximity_Backend.h" +#include "AP_Proximity_Backend_Serial.h" #include ///< for UARTDriver -class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend +class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial { public: - // constructor - AP_Proximity_RPLidarA2(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); - // static detection function - static bool detect(); + using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial; // update state void update(void) override; @@ -79,7 +76,6 @@ private: void reset_rplidar(); // reply related variables - AP_HAL::UARTDriver *_uart; uint8_t _descriptor[7]; char _rp_systeminfo[63]; bool _descriptor_data;