AP_Windvane: add NMEA wind sensor type

libraries/AP_WindVane/AP_WindVane.cpp

@@ -21,6 +21,7 @@
 #include "AP_WindVane_Airspeed.h"
 #include "AP_WindVane_RPM.h"
 #include "AP_WindVane_SITL.h"
+#include "AP_WindVane_NMEA.h"
 
 #define WINDVANE_DEFAULT_PIN 15                     // default wind vane sensor analog pin
 #define WINDSPEED_DEFAULT_SPEED_PIN 14              // default pin for reading speed from ModernDevice rev p wind sensor
@@ -32,7 +33,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Wind Vane Type
     // @Description: Wind Vane type
-    // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog
+    // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,10:SITL
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE),
@@ -114,7 +115,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = {
     // @Param: SPEED_TYPE
     // @DisplayName: Wind speed sensor Type
     // @Description: Wind speed sensor type
-    // @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,10:SITL
+    // @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,4:NMEA,10:SITL
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type,  0),
@@ -179,9 +180,9 @@ bool AP_WindVane::enabled() const
 }
 
 // Initialize the Wind Vane object and prepare it for use
-void AP_WindVane::init()
+void AP_WindVane::init(const AP_SerialManager& serial_manager)
 {
-    // don't init twice
+    // don't construct twice
     if (_direction_driver != nullptr || _speed_driver != nullptr ) {
         return;
     }
@@ -203,6 +204,10 @@ void AP_WindVane::init()
             _direction_driver = new AP_WindVane_SITL(*this);
 #endif
             break;
+        case WindVaneType::WINDVANE_NMEA:
+            _direction_driver = new AP_WindVane_NMEA(*this);
+            _direction_driver->init(serial_manager);
+            break;
     }
 
     // wind speed
@@ -218,13 +223,22 @@ void AP_WindVane::init()
         case Speed_type::WINDSPEED_SITL:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
             // single driver does both speed and direction
-            if (_direction_type != WINDVANE_SITL) {
+            if (_direction_type != WindVaneType::WINDVANE_SITL) {
                 _speed_driver = new AP_WindVane_SITL(*this);
             } else {
                 _speed_driver = _direction_driver;
             }
 #endif
             break;
+        case Speed_type::WINDSPEED_NMEA:
+            // single driver does both speed and direction
+            if (_direction_type != WindVaneType::WINDVANE_NMEA) {
+                _speed_driver = new AP_WindVane_NMEA(*this);
+                _speed_driver->init(serial_manager);
+            } else {
+                _speed_driver = _direction_driver;
+            }
+            break;
         case Speed_type::WINDSPEED_RPM:
             _speed_driver = new AP_WindVane_RPM(*this);
             break;

libraries/AP_WindVane/AP_WindVane.h

@@ -16,13 +16,12 @@
 
 #include <AP_Param/AP_Param.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_SerialManager/AP_SerialManager.h>
 
 class AP_WindVane_Backend;
 
 class AP_WindVane
 {
-
-public:
     friend class AP_WindVane_Backend;
     friend class AP_WindVane_Home;
     friend class AP_WindVane_Analog;
@@ -30,7 +29,9 @@ public:
     friend class AP_WindVane_ModernDevice;
     friend class AP_WindVane_Airspeed;
     friend class AP_WindVane_RPM;
+    friend class AP_WindVane_NMEA;
 
+public:
     AP_WindVane();
 
     /* Do not allow copies */
@@ -43,7 +44,7 @@ public:
     bool enabled() const;
 
     // Initialize the Wind Vane object and prepare it for use
-    void init();
+    void init(const AP_SerialManager& serial_manager);
 
     // update wind vane
     void update();
@@ -122,6 +123,7 @@ private:
         WINDVANE_HOME_HEADING   = 1,
         WINDVANE_PWM_PIN        = 2,
         WINDVANE_ANALOG_PIN     = 3,
+        WINDVANE_NMEA           = 4,
         WINDVANE_SITL           = 10
     };
 
@@ -130,6 +132,7 @@ private:
         WINDSPEED_AIRSPEED           = 1,
         WINDVANE_WIND_SENSOR_REV_P   = 2,
         WINDSPEED_RPM                = 3,
+        WINDSPEED_NMEA               = 4,
         WINDSPEED_SITL               = 10
     };
 

libraries/AP_WindVane/AP_WindVane_Backend.h

@@ -29,6 +29,9 @@ public:
     // override with a custom destructor if need be
     virtual ~AP_WindVane_Backend() {}
 
+    // initialization
+    virtual void init(const AP_SerialManager& serial_manager) {};
+
     // update the state structure
     virtual void update_speed() {};
     virtual void update_direction() {};

libraries/AP_WindVane/AP_WindVane_NMEA.cpp

@@ -0,0 +1,210 @@
+/*
+   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/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_WindVane_NMEA.h"
+#include <AP_SerialManager/AP_SerialManager.h>
+
+/*
+    NMEA wind vane library, tested with Calypso Wired sensor,
+    should also work with other NMEA wind sensors using the MWV message,
+    heavily based on RangeFinder NMEA library
+*/
+
+// constructor
+AP_WindVane_NMEA::AP_WindVane_NMEA(AP_WindVane &frontend) :
+    AP_WindVane_Backend(frontend)
+{
+}
+
+// init - performs any required initialization for this instance
+void AP_WindVane_NMEA::init(const AP_SerialManager& serial_manager)
+{
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_WindVane, 0);
+    if (uart != nullptr) {
+        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_WindVane, 0));
+    }
+}
+
+void AP_WindVane_NMEA::update_direction()
+{
+    // Only call update from here if it has not been called already by update speed
+    if (_frontend._speed_sensor_type.get() != _frontend.Speed_type::WINDSPEED_NMEA) {
+        update();
+    }
+}
+
+void AP_WindVane_NMEA::update_speed()
+{
+    update();
+}
+
+void AP_WindVane_NMEA::update()
+{
+    if (uart == nullptr) {
+        return;
+    }
+
+    // read any available lines from the windvane
+    int16_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        char c = uart->read();
+        if (decode(c)) {
+            // user may not have NMEA selected for both speed and direction
+            if (_frontend._direction_type.get() == _frontend.WindVaneType::WINDVANE_NMEA) {
+                direction_update_frontend(wrap_PI(radians(_wind_dir_deg + _frontend._dir_analog_bearing_offset.get())));
+            }
+            if (_frontend._speed_sensor_type.get() == _frontend.Speed_type::WINDSPEED_NMEA) {
+                speed_update_frontend(_speed_ms);
+            }
+        }
+    }
+}
+
+// add a single character to the buffer and attempt to decode
+// returns true if a complete sentence was successfully decoded
+bool AP_WindVane_NMEA::decode(char c)
+{
+    switch (c) {
+    case ',':
+        // end of a term, add to checksum
+        _checksum ^= c;
+        FALLTHROUGH;
+    case '\r':
+    case '\n':
+    case '*':
+    {
+        // 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;
+    }
+
+    case '$': // sentence begin
+        _sentence_valid = false;
+        _term_number = 0;
+        _term_offset = 0;
+        _checksum = 0;
+        _term_is_checksum = false;
+        _wind_dir_deg = -1.0f;
+        _speed_ms = -1.0f;
+        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_WindVane_NMEA::decode_latest_term()
+{
+    // handle the last term in a message
+    if (_term_is_checksum) {
+        uint8_t checksum = 16 * char_to_hex(_term[0]) + char_to_hex(_term[1]);
+        return ((checksum == _checksum) && _sentence_valid);
+    }
+
+    // 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.
+        if (_term[0] < 'A' || _term[0] > 'Z' ||
+            _term[1] < 'A' || _term[1] > 'Z') {
+             // unknown ID (we are actually expecting II)
+            return false;
+        }
+        const char *term_type = &_term[2];
+        if (strcmp(term_type, "MWV") == 0) {
+            // we found the sentence type for wind
+            _sentence_valid = true;
+        }
+        return false;
+    }
+
+    // if this is not the sentence we want then wait for another
+    if (!_sentence_valid) {
+        return false;
+    }
+
+    switch (_term_number) {
+        case 1:
+            _wind_dir_deg = atof(_term);
+            // check for sensible value
+            if (is_negative(_wind_dir_deg) || _wind_dir_deg > 360.0f) {
+                _sentence_valid = false;
+            }
+            break;
+
+        case 2:
+            // we are expecting R for relative wind 
+            // (could be T for true wind, maybe add in the future...)
+            if (_term[0] != 'R') {
+                _sentence_valid = false;
+            }
+            break;
+
+        case 3:
+            _speed_ms = atof(_term);
+            break;
+
+        case 4:
+            if (_term[0] == 'K') {
+                // convert from km/h to m/s
+                _speed_ms *= KM_PER_HOUR_TO_M_PER_SEC;
+            } else if (_term[0] == 'N') {
+                // convert from Knots to m/s
+                _speed_ms *= KNOTS_TO_M_PER_SEC;
+            }
+            // could also be M for m/s, but we want that anyway so nothing to do
+            // check for sensible value
+            if (is_negative(_speed_ms) || _speed_ms > 100.0f) {
+                _sentence_valid = false;
+            }
+            break;
+
+        case 5:
+            // expecting A for data valid
+            if (_term[0] != 'A') {
+                _sentence_valid = false;
+            }
+            break;
+
+    }
+    return false;
+}
+
+// return the numeric value of an ascii hex character
+int16_t AP_WindVane_NMEA::char_to_hex(char a)
+{
+    if (a >= 'A' && a <= 'F')
+        return a - 'A' + 10;
+    else if (a >= 'a' && a <= 'f')
+        return a - 'a' + 10;
+    else
+        return a - '0';
+}

libraries/AP_WindVane/AP_WindVane_NMEA.h

@@ -0,0 +1,58 @@
+/*
+   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/>.
+ */
+#pragma once
+
+#include "AP_WindVane_Backend.h"
+
+class AP_WindVane_NMEA : public AP_WindVane_Backend
+{
+public:
+    // constructor
+    AP_WindVane_NMEA(AP_WindVane &frontend);
+
+    // initialization
+    void init(const AP_SerialManager& serial_manager) override;
+
+    // update state
+    void update_direction() override;
+    void update_speed() override;
+
+private:
+    // pointer to serial uart
+    AP_HAL::UARTDriver *uart = nullptr; 
+
+    // See if we can read in some data
+    void update();
+
+    // try and decode NMEA message
+    bool decode(char c);
+
+    // decode each term
+    bool decode_latest_term();
+
+    // convert from char to hex value for checksum
+    int16_t char_to_hex(char a);
+
+    // latest values read in
+    float _speed_ms;
+    float _wind_dir_deg;
+
+    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
+    uint8_t _checksum;         // checksum accumulator
+    bool _term_is_checksum;    // current term is the checksum
+    bool _sentence_valid;      // is current sentence valid so far
+};