diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index aa12b79974..a13d3cfd8f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -13,36 +13,13 @@ along with this program. If not, see . */ -#include -#include "AP_RangeFinder_LightWareSerial.h" -#include -#include #include "AP_RangeFinder_NMEA.h" +#include +#include + extern const AP_HAL::HAL& hal; -// constructor initialises the rangefinder -// Note this is called after detect() returns true, so we -// already know that we should setup the rangefinder -AP_RangeFinder_NMEA::AP_RangeFinder_NMEA(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance) : - AP_RangeFinder_Backend(_state, _params), - _distance_m(-1.0f) -{ - 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 NMEA rangefinder by looking to see if the user has configured it -bool AP_RangeFinder_NMEA::detect(uint8_t serial_instance) -{ - return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr; -} - // update the state of the sensor void AP_RangeFinder_NMEA::update(void) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 743b7e8537..ca0d44f5c9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -16,19 +16,14 @@ #pragma once #include "RangeFinder.h" -#include "RangeFinder_Backend.h" +#include "RangeFinder_Backend_Serial.h" -class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend +class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend_Serial { public: - // constructor - AP_RangeFinder_NMEA(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; @@ -60,13 +55,11 @@ private: // returns true if new sentence has just passed checksum test and is validated bool decode_latest_term(); - AP_HAL::UARTDriver *uart = nullptr; // pointer to serial uart - // message decoding related members char _term[15]; // buffer for the current term within the current sentence uint8_t _term_offset; // offset within the _term buffer where the next character should be placed uint8_t _term_number; // term index within the current sentence - float _distance_m; // distance in meters parsed from a term, -1 if no distance + float _distance_m = -1.0f; // distance in meters parsed from a term, -1 if no distance uint8_t _checksum; // checksum accumulator bool _term_is_checksum; // current term is the checksum sentence_types _sentence_type; // the sentence type currently being processed