AP_RangeFinder: add get_temp and implement for NMEA driver

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
 {

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.

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

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) &&
+        if (checksum == _checksum) {
-                !is_negative(_distance_m) &&
+            if ((_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT) && !is_negative(_distance_m)) {
-                (_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT));
+                // 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;

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