From 0246dd990ee0749c2fbc82f71d59c3e0518ff68d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Nov 2019 17:29:03 +1100 Subject: [PATCH] AP_RangeFinder: make MaxsonarSerialLV backend use new intermediate class --- .../AP_RangeFinder_MaxsonarSerialLV.cpp | 33 ++----------------- .../AP_RangeFinder_MaxsonarSerialLV.h | 12 ++----- 2 files changed, 5 insertions(+), 40 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 4e28464801..d89d611a6e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -15,42 +15,15 @@ * with this program. If not, see . */ -#include -#include -#include #include "AP_RangeFinder_MaxsonarSerialLV.h" +#include +#include + #define MAXSONAR_SERIAL_LV_BAUD_RATE 9600 extern const AP_HAL::HAL& hal; -/* - The constructor also initialises the rangefinder. Note that this - constructor is not called until detect() returns true, so we - already know that we should setup the rangefinder -*/ -AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance) : - AP_RangeFinder_Backend(_state, _params) -{ - const AP_SerialManager &serial_manager = AP::serialmanager(); - uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); - if (uart != nullptr) { - uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)); - } -} - -/* - detect if a MaxSonar rangefinder is connected. We'll detect by - trying to take a reading on Serial. If we get a result the sensor is - there. -*/ -bool AP_RangeFinder_MaxsonarSerialLV::detect(uint8_t serial_instance) -{ - return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; -} - // read - return last value measured by sensor bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index 287a360f3e..0866e7689c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h @@ -1,19 +1,12 @@ #pragma once #include "RangeFinder.h" -#include "RangeFinder_Backend.h" +#include "RangeFinder_Backend_Serial.h" -class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend +class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend_Serial { public: - // constructor - AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance); - - // static detection function - static bool detect(uint8_t serial_instance); // update state void update(void) override; @@ -28,7 +21,6 @@ private: // get a reading bool get_reading(uint16_t &reading_cm); - AP_HAL::UARTDriver *uart = nullptr; char linebuf[10]; uint8_t linebuf_len = 0; };