From 60cbefc1aeb0cd7d06046318ef81d9b32d488065 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Nov 2019 21:59:50 +1100 Subject: [PATCH] AP_RangeFinder: move update function to serial base class --- libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_Benewake.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 2 +- .../AP_RangeFinder_LightWareSerial.h | 2 +- .../AP_RangeFinder_MaxsonarSerialLV.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_uLanding.h | 2 +- .../AP_RangeFinder/RangeFinder_Backend_Serial.cpp | 15 +++++++++++++++ .../AP_RangeFinder/RangeFinder_Backend_Serial.h | 7 +++++++ 11 files changed, 31 insertions(+), 9 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h index a496fa2e51..46ac8b8100 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h @@ -27,7 +27,7 @@ private: void send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len); // read a distance from the sensor - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; // process one byte received on serial port // returns true if a distance message has been successfully parsed diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h index 4515c944a7..2d9b90df8e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h @@ -26,7 +26,7 @@ private: // get a reading // distance returned in reading_cm - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; uint8_t linebuf[10]; uint8_t linebuf_len; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h index 7cabe90af2..7e88d13a82 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h @@ -26,7 +26,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; uint8_t buf[6]; uint8_t buf_len = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 9a488bf51f..d67cf4028c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -55,7 +55,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; // CRC16 bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index c462f2d50a..cb00f68204 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h @@ -21,7 +21,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; char linebuf[10]; uint8_t linebuf_len = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index 7d83aa549e..bface028d4 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h @@ -21,7 +21,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; char linebuf[10]; uint8_t linebuf_len = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index ca0d44f5c9..af64a6ac86 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -44,7 +44,7 @@ private: }; // get a reading - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; // add a single character to the buffer and attempt to decode // returns true if a complete sentence was successfully decoded diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h index ac2727a825..7f00e30a1b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h @@ -47,7 +47,7 @@ private: wasp_configuration_stage configuration_state = WASP_CFG_PROTOCOL; - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; void parse_response(void); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index 3c6c5caf61..2cd6f6fd79 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h @@ -32,7 +32,7 @@ private: bool detect_version(void); // get a reading - bool get_reading(uint16_t &reading_cm); + bool get_reading(uint16_t &reading_cm) override; uint8_t _linebuf[6]; uint8_t _linebuf_len; diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp index 3d8ddb2ed0..30bc07f3c1 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp @@ -51,3 +51,18 @@ bool AP_RangeFinder_Backend_Serial::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_Backend_Serial::update(void) +{ + if (get_reading(state.distance_cm)) { + // update range_valid state based on distance measured + state.last_reading_ms = AP_HAL::millis(); + update_status(); + } else if (AP_HAL::millis() - state.last_reading_ms > 200) { + set_status(RangeFinder::Status::NoData); + } +} diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h index e8f8b26e37..371ef77175 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h @@ -23,4 +23,11 @@ protected: virtual uint16_t tx_bufsize() const { return 0; } AP_HAL::UARTDriver *uart = nullptr; + + // update state; not all backends call this! + virtual void update(void) override; + + // it is essential that anyone relying on the base-class update to + // implement this: + virtual bool get_reading(uint16_t &reading_cm) { return 0; } };