From 49003868fefbf29fbc48f86361e7175325ac2d3b Mon Sep 17 00:00:00 2001 From: Peter Hall <33176108+IamPete1@users.noreply.github.com> Date: Tue, 28 May 2019 22:31:34 +0100 Subject: [PATCH] AP_Windvane: add NMEA wind sensor type --- libraries/AP_WindVane/AP_WindVane.cpp | 24 ++- libraries/AP_WindVane/AP_WindVane.h | 19 +- libraries/AP_WindVane/AP_WindVane_Backend.h | 3 + libraries/AP_WindVane/AP_WindVane_NMEA.cpp | 210 ++++++++++++++++++++ libraries/AP_WindVane/AP_WindVane_NMEA.h | 58 ++++++ 5 files changed, 301 insertions(+), 13 deletions(-) create mode 100644 libraries/AP_WindVane/AP_WindVane_NMEA.cpp create mode 100644 libraries/AP_WindVane/AP_WindVane_NMEA.h diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index f85975581b..7c4bb5979c 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/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; diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index ec3e82fa8b..81e5aab946 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -16,13 +16,12 @@ #include #include +#include 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(); @@ -118,11 +119,12 @@ private: float _home_heading; enum WindVaneType { - WINDVANE_NONE = 0, - WINDVANE_HOME_HEADING = 1, - WINDVANE_PWM_PIN = 2, - WINDVANE_ANALOG_PIN = 3, - WINDVANE_SITL = 10 + WINDVANE_NONE = 0, + WINDVANE_HOME_HEADING = 1, + WINDVANE_PWM_PIN = 2, + WINDVANE_ANALOG_PIN = 3, + WINDVANE_NMEA = 4, + WINDVANE_SITL = 10 }; enum Speed_type { @@ -130,6 +132,7 @@ private: WINDSPEED_AIRSPEED = 1, WINDVANE_WIND_SENSOR_REV_P = 2, WINDSPEED_RPM = 3, + WINDSPEED_NMEA = 4, WINDSPEED_SITL = 10 }; diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h index 56e1d73138..2fdb3a4656 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.h +++ b/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() {}; diff --git a/libraries/AP_WindVane/AP_WindVane_NMEA.cpp b/libraries/AP_WindVane/AP_WindVane_NMEA.cpp new file mode 100644 index 0000000000..cc690218ae --- /dev/null +++ b/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 . + */ + +#include +#include "AP_WindVane_NMEA.h" +#include + +/* + 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'; +} diff --git a/libraries/AP_WindVane/AP_WindVane_NMEA.h b/libraries/AP_WindVane/AP_WindVane_NMEA.h new file mode 100644 index 0000000000..aedae3eb67 --- /dev/null +++ b/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 . + */ +#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 +};