From b18f1a8e2269ca13747f04afbc25b780f47b994f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Nov 2019 21:35:23 +1100 Subject: [PATCH] AP_RangeFinder: move uLanding to using common serial backend --- .../AP_RangeFinder_uLanding.cpp | 31 ------------------- .../AP_RangeFinder/AP_RangeFinder_uLanding.h | 20 ++++++------ .../RangeFinder_Backend_Serial.cpp | 2 +- .../RangeFinder_Backend_Serial.h | 4 +++ 4 files changed, 16 insertions(+), 41 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index cf7854b120..dc2d13781b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp @@ -15,44 +15,13 @@ #include #include "AP_RangeFinder_uLanding.h" -#include #include #define ULANDING_HDR 254 // Header Byte from uLanding (0xFE) #define ULANDING_HDR_V0 72 // Header Byte for beta V0 of uLanding (0x48) -#define ULANDING_BAUD 115200 -#define ULANDING_BUFSIZE_RX 128 -#define ULANDING_BUFSIZE_TX 128 - 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_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance) : - AP_RangeFinder_Backend(_state, _params) -{ - uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); - if (uart != nullptr) { - uart->begin(ULANDING_BAUD, ULANDING_BUFSIZE_RX, ULANDING_BUFSIZE_TX); - } -} - -/* - detect if a uLanding 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_uLanding::detect(uint8_t serial_instance) -{ - return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; -} - /* detect uLanding Firmware Version */ diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index c76410aab3..3c6c5caf61 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h @@ -1,19 +1,14 @@ #pragma once #include "RangeFinder.h" -#include "RangeFinder_Backend.h" +#include "RangeFinder_Backend_Serial.h" -class AP_RangeFinder_uLanding : public AP_RangeFinder_Backend +class AP_RangeFinder_uLanding : public AP_RangeFinder_Backend_Serial { public: - // constructor - AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance); - // static detection function - static bool detect(uint8_t serial_instance); + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; // update state void update(void) override; @@ -24,6 +19,14 @@ protected: return MAV_DISTANCE_SENSOR_RADAR; } + // baudrate used during object construction: + uint32_t initial_baudrate(uint8_t serial_instance) const override { + return 115200; + } + + uint16_t rx_bufsize() const override { return 128; } + uint16_t tx_bufsize() const override { return 128; } + private: // detect uLanding Firmware Version bool detect_version(void); @@ -31,7 +34,6 @@ private: // get a reading bool get_reading(uint16_t &reading_cm); - AP_HAL::UARTDriver *uart; uint8_t _linebuf[6]; uint8_t _linebuf_len; bool _version_known; diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp index 01eaee5631..3d8ddb2ed0 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp @@ -34,7 +34,7 @@ AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial( { uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); if (uart != nullptr) { - uart->begin(initial_baudrate(serial_instance)); + uart->begin(initial_baudrate(serial_instance), rx_bufsize(), tx_bufsize()); } } diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h index 2d2922a736..e8f8b26e37 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h @@ -18,5 +18,9 @@ protected: // baudrate used during object construction: virtual uint32_t initial_baudrate(uint8_t serial_instance) const; + // the value 0 is special to the UARTDriver - it's "use default" + virtual uint16_t rx_bufsize() const { return 0; } + virtual uint16_t tx_bufsize() const { return 0; } + AP_HAL::UARTDriver *uart = nullptr; };