From fee30b444770141a242fa96154d4c5639e38e1be Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 13 Apr 2021 17:07:06 +0900 Subject: [PATCH] AP_RangeFinder: add get_temp and implement for NMEA driver --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 10 +++++ libraries/AP_RangeFinder/AP_RangeFinder.h | 3 ++ .../AP_RangeFinder/AP_RangeFinder_Backend.h | 3 ++ .../AP_RangeFinder/AP_RangeFinder_NMEA.cpp | 39 ++++++++++++++++--- .../AP_RangeFinder/AP_RangeFinder_NMEA.h | 13 +++++-- 5 files changed, 60 insertions(+), 8 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index c487eb2eba..9a7ba82633 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -744,6 +744,16 @@ MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotati return backend->get_mav_distance_sensor_type(); } +// get temperature reading in C. returns true on success and populates temp argument +bool RangeFinder::get_temp(enum Rotation orientation, float &temp) +{ + AP_RangeFinder_Backend *backend = find_instance(orientation); + if (backend == nullptr) { + return false; + } + return backend->get_temp(temp); +} + // Write an RFND (rangefinder) packet void RangeFinder::Log_RFND() const { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 8f805e7786..518a3cca9b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -181,6 +181,9 @@ public: const Vector3f &get_pos_offset_orient(enum Rotation orientation) const; uint32_t last_reading_ms(enum Rotation orientation) const; + // get temperature reading in C. returns true on success and populates temp argument + bool get_temp(enum Rotation orientation, float &temp); + /* set an externally estimated terrain height. Used to enable power saving (where available) at high altitudes. diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 7be7ff8257..873a615859 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -59,6 +59,9 @@ public: // return system time of last successful read from the sensor uint32_t last_reading_ms() const { return state.last_reading_ms; } + // get temperature reading in C. returns true on success and populates temp argument + virtual bool get_temp(float &temp) { return false; } + protected: // update status based on distance measurement diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index a15d925e0e..266f63c19e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -49,8 +49,19 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) return true; } +// get temperature reading +bool AP_RangeFinder_NMEA::get_temp(float &temp) +{ + uint32_t now_ms = AP_HAL::millis(); + if ((_temp_readtime_ms == 0) || ((now_ms - _temp_readtime_ms) > read_timeout_ms())) { + return false; + } + temp = _temp; + return true; +} + // add a single character to the buffer and attempt to decode -// returns true if a complete sentence was successfully decoded +// returns true if a distance was successfully decoded bool AP_RangeFinder_NMEA::decode(char c) { switch (c) { @@ -100,7 +111,7 @@ bool AP_RangeFinder_NMEA::decode(char c) } // decode the most recently consumed term -// returns true if new sentence has just passed checksum test and is validated +// returns true if new distance sentence has just passed checksum test and is validated bool AP_RangeFinder_NMEA::decode_latest_term() { // handle the last term in a message @@ -112,9 +123,20 @@ bool AP_RangeFinder_NMEA::decode_latest_term() return false; } const uint8_t checksum = (nibble_high << 4u) | nibble_low; - return ((checksum == _checksum) && - !is_negative(_distance_m) && - (_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT)); + if (checksum == _checksum) { + if ((_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT) && !is_negative(_distance_m)) { + // return true if distance is valid + return true; + } + if (_sentence_type == SONAR_MTW) { + _temp = _temp_unvalidated; + _temp_readtime_ms = AP_HAL::millis(); + // return false because this is not a distance + // temperature is accessed via separate accessor + return false; + } + } + return false; } // the first term determines the sentence type @@ -131,6 +153,8 @@ bool AP_RangeFinder_NMEA::decode_latest_term() _sentence_type = SONAR_DBT; } else if (strcmp(term_type, "DPT") == 0) { _sentence_type = SONAR_DPT; + } else if (strcmp(term_type, "MTW") == 0) { + _sentence_type = SONAR_MTW; } else { _sentence_type = SONAR_UNKNOWN; } @@ -147,6 +171,11 @@ bool AP_RangeFinder_NMEA::decode_latest_term() if (_term_number == 1) { _distance_m = strtof(_term, NULL); } + } else if (_sentence_type == SONAR_MTW) { + // parse MTW (mean water temperature) messages + if (_term_number == 1) { + _temp_unvalidated = strtof(_term, NULL); + } } return false; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 81d1636fed..52437c82ed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -37,16 +37,20 @@ private: enum sentence_types : uint8_t { SONAR_UNKNOWN = 0, SONAR_DBT, - SONAR_DPT + SONAR_DPT, + SONAR_MTW // mean water temperature }; - // get a reading + // get a distance reading bool get_reading(uint16_t &reading_cm) override; + // get temperature reading in C. returns true on success and populates temp argument + bool get_temp(float &temp) override; + uint16_t read_timeout_ms() const override { return 3000; } // add a single character to the buffer and attempt to decode - // returns true if a complete sentence was successfully decoded + // returns true if a distance was successfully decoded // distance should be pulled directly from _distance_m member bool decode(char c); @@ -59,6 +63,9 @@ private: 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 = -1.0f; // distance in meters parsed from a term, -1 if no distance + float _temp_unvalidated; // unvalidated temperature in C (may have failed checksum) + float _temp; // temperature in C (validated) + uint32_t _temp_readtime_ms; // system time we last read a validated temperature, 0 if never read uint8_t _checksum; // checksum accumulator bool _term_is_checksum; // current term is the checksum sentence_types _sentence_type; // the sentence type currently being processed