AP_Airspeed: add NMEA water speed backend

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -24,6 +24,7 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_Logger/AP_Logger.h>
 #include <utility>
+#include <AP_Vehicle/AP_Vehicle.h>
 #include "AP_Airspeed.h"
 #include "AP_Airspeed_MS4525.h"
 #include "AP_Airspeed_MS5525.h"
@@ -34,7 +35,9 @@
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
 #include "AP_Airspeed_UAVCAN.h"
 #endif
-
+#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
+#include "AP_Airspeed_NMEA.h"
+#endif
 extern const AP_HAL::HAL &hal;
 
 #ifdef HAL_AIRSPEED_TYPE_DEFAULT
@@ -77,7 +80,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Airspeed type
     // @Description: Type of airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE),
 
@@ -184,7 +187,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Second Airspeed type
     // @Description: Type of 2nd airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed
     // @User: Standard
     AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE),
 
@@ -353,6 +356,11 @@ void AP_Airspeed::init()
         case TYPE_UAVCAN:
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
             sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i);
+#endif
+            break;
+        case TYPE_NMEA_WATER:
+#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
+            sensor[i] = new AP_Airspeed_NMEA(*this, i);
 #endif
             break;
         }
@@ -457,17 +465,24 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
 // read one airspeed sensor
 void AP_Airspeed::read(uint8_t i)
 {
-    float airspeed_pressure;
     if (!enabled(i) || !sensor[i]) {
         return;
     }
+    state[i].last_update_ms = AP_HAL::millis();
+
+    // try and get a direct reading of airspeed
+    if (sensor[i]->has_airspeed()) {
+        state[i].healthy = sensor[i]->get_airspeed(state[i].airspeed);
+        return;
+    }
+
     bool prev_healthy = state[i].healthy;
     float raw_pressure = get_pressure(i);
     if (state[i].cal.start_ms != 0) {
         update_calibration(i, raw_pressure);
     }
-    
-    airspeed_pressure = raw_pressure - param[i].offset;
+
+    float airspeed_pressure = raw_pressure - param[i].offset;
 
     // remember raw pressure for logging
     state[i].corrected_pressure = airspeed_pressure;
@@ -511,7 +526,6 @@ void AP_Airspeed::read(uint8_t i)
         state[i].healthy = false;
     }
 
-    state[i].last_update_ms = AP_HAL::millis();
 }
 
 // read all airspeed sensors

libraries/AP_Airspeed/AP_Airspeed.h

@@ -156,6 +156,7 @@ public:
         TYPE_I2C_DLVR_20IN=10,
         TYPE_I2C_DLVR_30IN=11,
         TYPE_I2C_DLVR_60IN=12,
+        TYPE_NMEA_WATER=13,
     };
 
     // get current primary sensor

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -31,11 +31,17 @@ public:
     virtual bool init(void) = 0;
 
     // return the current differential_pressure in Pascal
-    virtual bool get_differential_pressure(float &pressure) = 0;
+    virtual bool get_differential_pressure(float &pressure) {return false;}
 
     // return the current temperature in degrees C, if available
     virtual bool get_temperature(float &temperature) = 0;
 
+    // true if sensor reads airspeed directly, not via pressue
+    virtual bool has_airspeed() {return false;}
+
+    // return airspeed in m/s if available
+    virtual bool get_airspeed(float& airspeed) {return false;}
+
 protected:
     int8_t get_pin(void) const;
     float get_psi_range(void) const;
@@ -44,7 +50,7 @@ protected:
     AP_Airspeed::pitot_tube_order get_tube_order(void) const {
         return AP_Airspeed::pitot_tube_order(frontend.param[instance].tube_order.get());
     }
-    
+
     // semaphore for access to shared frontend data
     HAL_Semaphore sem;
 
@@ -66,7 +72,12 @@ protected:
     void set_offset(float ofs) {
         frontend.param[instance].offset.set(ofs);
     }
-    
+
+    // set use
+    void set_use(int8_t use) {
+        frontend.param[instance].use.set(use);
+    }
+
 private:
     AP_Airspeed &frontend;
     uint8_t instance;

libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp

@@ -0,0 +1,218 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+ *   NMEA Sensor driver for VHW ans MTW messages over Serial
+ *   https://gpsd.gitlab.io/gpsd/NMEA.html#_vhw_water_speed_and_heading
+ *   https://gpsd.gitlab.io/gpsd/NMEA.html#_mtw_mean_temperature_of_water
+ */
+
+#include <AP_Vehicle/AP_Vehicle.h>
+#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
+
+#include "AP_Airspeed_NMEA.h"
+#include "AP_Airspeed.h"
+
+#define TIMEOUT_MS 2000
+
+extern const AP_HAL::HAL &hal;
+
+AP_Airspeed_NMEA::AP_Airspeed_NMEA(AP_Airspeed &_frontend, uint8_t _instance) :
+    AP_Airspeed_Backend(_frontend, _instance)
+{
+}
+
+bool AP_Airspeed_NMEA::init()
+{
+    const AP_SerialManager& serial_manager = AP::serialmanager();
+
+    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_AirSpeed, 0);
+    if (_uart == nullptr) {
+        return false;
+    }
+
+    _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_AirSpeed, 0));
+
+    // make sure this sensor cannot be used in the EKF
+    set_use(0);
+
+    // must set use zero offset to pass offset check for health
+    set_use_zero_offset();
+
+    return true;
+}
+
+// read the from the sensor
+bool AP_Airspeed_NMEA::get_airspeed(float &airspeed)
+{
+    if (_uart == nullptr) {
+        return false;
+    }
+
+    uint32_t now = AP_HAL::millis();
+
+    // read any available lines from the sensor
+    float sum = 0.0f;
+    uint16_t count = 0;
+    int16_t nbytes = _uart->available();
+    while (nbytes-- > 0) {
+        char c = _uart->read();
+        if (decode(c)) {
+            _last_update_ms = now;
+            if (_sentence_type == TYPE_VHW) {
+                sum += _speed;
+                count++;
+            } else {
+                _temp_sum += _temp;
+                _temp_count++;
+            }
+        }
+    }
+
+    if (count == 0) {
+        // Cant return false because updates are too slow, return previous reading
+        // Could return false after some timeout, however testing shows that the DST800 just stops sending the message at zero speed
+        airspeed = _last_speed;
+    } else {
+        // return average of all measurements
+        airspeed = sum / count;
+        _last_speed = airspeed;
+    }
+
+    return (now - _last_update_ms) < TIMEOUT_MS;
+}
+
+// return the current temperature in degrees C
+// the main update is done in the get_pressue function
+// this just reports the value
+bool AP_Airspeed_NMEA::get_temperature(float &temperature)
+{
+    if (_uart == nullptr) {
+        return false;
+    }
+
+    if (_temp_count == 0) {
+        temperature = _last_temp;
+    } else {
+        // return average of all measurements
+        temperature = _temp_sum / _temp_count;
+        _last_temp = temperature;
+        _temp_count = 0;
+        _temp_sum = 0;
+    }
+
+    return true;
+}
+
+
+// add a single character to the buffer and attempt to decode
+// returns true if a complete sentence was successfully decoded
+bool AP_Airspeed_NMEA::decode(char c)
+{
+    switch (c) {
+    case ',':
+        // end of a term, add to checksum
+        _checksum ^= c;
+        FALLTHROUGH;
+    case '\r':
+    case '\n':
+    case '*':
+    {
+        if (!_sentence_done && _sentence_valid) {
+            // null terminate and decode latest term
+            _term[_term_offset] = 0;
+            bool valid_sentence = decode_latest_term();
+
+            // move onto next term
+            _term_number++;
+            _term_offset = 0;
+            _term_is_checksum = (c == '*');
+            return valid_sentence;
+        }
+        return false;
+    }
+
+    case '$': // sentence begin
+        _term_number = 0;
+        _term_offset = 0;
+        _checksum = 0;
+        _term_is_checksum = false;
+        _sentence_done = false;
+        _sentence_valid = true;
+        return false;
+    }
+
+    // ordinary characters are added to term
+    if (_term_offset < sizeof(_term) - 1) {
+        _term[_term_offset++] = c;
+    }
+    if (!_term_is_checksum) {
+        _checksum ^= c;
+    }
+
+    return false;
+}
+
+// decode the most recently consumed term
+// returns true if new sentence has just passed checksum test and is validated
+bool AP_Airspeed_NMEA::decode_latest_term()
+{
+    // handle the last term in a message
+    if (_term_is_checksum) {
+        _sentence_done = true;
+        uint8_t nibble_high = 0;
+        uint8_t nibble_low  = 0;
+        if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) {
+            return false;
+        }
+        const uint8_t checksum = (nibble_high << 4u) | nibble_low;
+        return checksum == _checksum;
+    }
+
+    // the first term determines the sentence type
+    if (_term_number == 0) {
+        // the first two letters of the NMEA term are the talker ID.
+        // we accept any two characters here.
+        // actually expecting YX for MTW and VW for VHW
+        if (_term[0] < 'A' || _term[0] > 'Z' ||
+            _term[1] < 'A' || _term[1] > 'Z') {
+            return false;
+        }
+        const char *term_type = &_term[2];
+        if (strcmp(term_type, "MTW") == 0) {
+            _sentence_type = TPYE_MTW;
+        } else if (strcmp(term_type, "VHW") == 0) {
+            _sentence_type = TYPE_VHW;
+        } else {
+            _sentence_valid = false;
+        }
+        return false;
+    }
+
+    if (_sentence_type == TPYE_MTW) {
+        // parse MTW messages
+        if (_term_number == 1) {
+            _temp = strtof(_term, NULL);
+        }
+    } else if (_sentence_type == TYPE_VHW) {
+        // parse VHW messages
+        if (_term_number == 7) {
+            _speed = strtof(_term, NULL) * KM_PER_HOUR_TO_M_PER_SEC;
+        }
+    }
+
+    return false;
+}
+
+#endif  // APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 

libraries/AP_Airspeed/AP_Airspeed_NMEA.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include "AP_Airspeed_Backend.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+
+class AP_Airspeed_NMEA : public AP_Airspeed_Backend
+{
+public:
+    AP_Airspeed_NMEA(AP_Airspeed &frontend, uint8_t _instance);
+
+    // probe and initialise the sensor
+    bool init(void) override;
+
+    // this reads airspeed directly
+    bool has_airspeed() override {return true;}
+
+    // read the from the sensor
+    bool get_airspeed(float &airspeed) override;
+
+    // return the current temperature in degrees C
+    bool get_temperature(float &temperature) override;
+
+
+private:
+    // pointer to serial uart
+    AP_HAL::UARTDriver *_uart = nullptr; 
+
+    // add a single character to the buffer and attempt to decode
+    // returns true if a complete sentence was successfully decoded
+    // distance should be pulled directly from _distance_m member
+    bool decode(char c);
+
+    // decode the just-completed term
+    // returns true if new sentence has just passed checksum test and is validated
+    bool decode_latest_term();
+
+    // enum for handled messages
+    enum sentence_types : uint8_t {
+        TPYE_MTW = 0,
+        TYPE_VHW,
+    };
+
+
+    // 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 _speed;                           // speed in m/s
+    float _temp;                            // temp in deg c
+    uint8_t _checksum;                      // checksum accumulator
+    bool _term_is_checksum;                 // current term is the checksum
+    bool _sentence_done;                    // has the current term already been decoded
+    bool _sentence_valid;                   // is the decodeing valid so far
+    sentence_types _sentence_type;          // the sentence type currently being processed
+
+    // Store the temp ready for a temp request
+    float _temp_sum;
+    uint16_t _temp_count;
+
+    // store last sent speed and temp as update rate is slow
+    float _last_temp;
+    float _last_speed;
+
+    // time last message was received
+    uint32_t _last_update_ms;
+};