Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_WindVane_config.h"
      17             : 
      18             : #if AP_WINDVANE_NMEA_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include "AP_WindVane_NMEA.h"
      22             : #include <AP_SerialManager/AP_SerialManager.h>
      23             : 
      24             : /*
      25             :     NMEA wind vane library, tested with Calypso Wired sensor,
      26             :     should also work with other NMEA wind sensors using the MWV message,
      27             :     heavily based on RangeFinder NMEA library
      28             : */
      29             : 
      30             : // init - performs any required initialization for this instance
      31           0 : void AP_WindVane_NMEA::init(const AP_SerialManager& serial_manager)
      32             : {
      33           0 :     uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_WindVane, 0);
      34           0 :     if (uart != nullptr) {
      35           0 :         uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_WindVane, 0));
      36             :     }
      37           0 : }
      38             : 
      39           0 : void AP_WindVane_NMEA::update_direction()
      40             : {
      41             :     // Only call update from here if it has not been called already by update speed
      42           0 :     if (_frontend._speed_sensor_type.get() != _frontend.Speed_type::WINDSPEED_NMEA) {
      43           0 :         update();
      44             :     }
      45           0 : }
      46             : 
      47           0 : void AP_WindVane_NMEA::update_speed()
      48             : {
      49           0 :     update();
      50           0 : }
      51             : 
      52           0 : void AP_WindVane_NMEA::update()
      53             : {
      54           0 :     if (uart == nullptr) {
      55           0 :         return;
      56             :     }
      57             : 
      58             :     // read any available lines from the windvane
      59           0 :     int16_t nbytes = uart->available();
      60           0 :     while (nbytes-- > 0) {
      61           0 :         char c = uart->read();
      62           0 :         if (decode(c)) {
      63             :             // user may not have NMEA selected for both speed and direction
      64           0 :             if (_frontend._direction_type.get() == _frontend.WindVaneType::WINDVANE_NMEA) {
      65           0 :                 _frontend._direction_apparent_raw = wrap_PI(radians(_wind_dir_deg + _frontend._dir_analog_bearing_offset.get()));
      66             :             }
      67           0 :             if (_frontend._speed_sensor_type.get() == _frontend.Speed_type::WINDSPEED_NMEA) {
      68           0 :                 _frontend._speed_apparent_raw = _speed_ms;
      69             :             }
      70             :         }
      71             :     }
      72             : }
      73             : 
      74             : // add a single character to the buffer and attempt to decode
      75             : // returns true if a complete sentence was successfully decoded
      76           0 : bool AP_WindVane_NMEA::decode(char c)
      77             : {
      78           0 :     switch (c) {
      79           0 :     case ',':
      80             :         // end of a term, add to checksum
      81           0 :         _checksum ^= c;
      82             :         FALLTHROUGH;
      83           0 :     case '\r':
      84             :     case '\n':
      85             :     case '*':
      86             :     {
      87           0 :         if (_sentence_done) {
      88           0 :             return false;
      89             :         }
      90             : 
      91             :         // null terminate and decode latest term
      92           0 :         _term[_term_offset] = 0;
      93           0 :         bool valid_sentence = decode_latest_term();
      94             : 
      95             :         // move onto next term
      96           0 :         _term_number++;
      97           0 :         _term_offset = 0;
      98           0 :         _term_is_checksum = (c == '*');
      99           0 :         return valid_sentence;
     100             :     }
     101             : 
     102           0 :     case '$': // sentence begin
     103           0 :         _sentence_valid = false;
     104           0 :         _term_number = 0;
     105           0 :         _term_offset = 0;
     106           0 :         _checksum = 0;
     107           0 :         _term_is_checksum = false;
     108           0 :         _wind_dir_deg = -1.0f;
     109           0 :         _speed_ms = -1.0f;
     110           0 :         _sentence_done = false;
     111           0 :         return false;
     112             :     }
     113             : 
     114             :     // ordinary characters are added to term
     115           0 :     if (_term_offset < sizeof(_term) - 1) {
     116           0 :         _term[_term_offset++] = c;
     117             :     }
     118           0 :     if (!_term_is_checksum) {
     119           0 :         _checksum ^= c;
     120             :     }
     121             : 
     122           0 :     return false;
     123             : }
     124             : 
     125             : // decode the most recently consumed term
     126             : // returns true if new sentence has just passed checksum test and is validated
     127           0 : bool AP_WindVane_NMEA::decode_latest_term()
     128             : {
     129             :     // handle the last term in a message
     130           0 :     if (_term_is_checksum) {
     131           0 :         _sentence_done = true;
     132             :         uint8_t checksum;
     133           0 :         if (!hex_twochars_to_uint8(_term, checksum)) {
     134           0 :             return false;
     135             :         }
     136           0 :         return ((checksum == _checksum) && _sentence_valid);
     137             :     }
     138             : 
     139             :     // the first term determines the sentence type
     140           0 :     if (_term_number == 0) {
     141             :         // the first two letters of the NMEA term are the talker ID.
     142             :         // we accept any two characters here.
     143           0 :         if (_term[0] < 'A' || _term[0] > 'Z' ||
     144           0 :             _term[1] < 'A' || _term[1] > 'Z') {
     145             :              // unknown ID (we are actually expecting II)
     146           0 :             return false;
     147             :         }
     148           0 :         const char *term_type = &_term[2];
     149           0 :         if (strcmp(term_type, "MWV") == 0) {
     150             :             // we found the sentence type for wind
     151           0 :             _sentence_valid = true;
     152             :         }
     153           0 :         return false;
     154             :     }
     155             : 
     156             :     // if this is not the sentence we want then wait for another
     157           0 :     if (!_sentence_valid) {
     158           0 :         return false;
     159             :     }
     160             : 
     161           0 :     switch (_term_number) {
     162           0 :         case 1:
     163           0 :             _wind_dir_deg = strtof(_term, NULL);
     164             :             // check for sensible value
     165           0 :             if (is_negative(_wind_dir_deg) || _wind_dir_deg > 360.0f) {
     166           0 :                 _sentence_valid = false;
     167             :             }
     168           0 :             break;
     169             : 
     170           0 :         case 2:
     171             :             // we are expecting R for relative wind 
     172             :             // (could be T for true wind, maybe add in the future...)
     173           0 :             if (_term[0] != 'R') {
     174           0 :                 _sentence_valid = false;
     175             :             }
     176           0 :             break;
     177             : 
     178           0 :         case 3:
     179           0 :             _speed_ms = strtof(_term, NULL);
     180           0 :             break;
     181             : 
     182           0 :         case 4:
     183           0 :             if (_term[0] == 'K') {
     184             :                 // convert from km/h to m/s
     185           0 :                 _speed_ms *= KM_PER_HOUR_TO_M_PER_SEC;
     186           0 :             } else if (_term[0] == 'N') {
     187             :                 // convert from Knots to m/s
     188           0 :                 _speed_ms *= KNOTS_TO_M_PER_SEC;
     189             :             }
     190             :             // could also be M for m/s, but we want that anyway so nothing to do
     191             :             // check for sensible value
     192           0 :             if (is_negative(_speed_ms) || _speed_ms > 100.0f) {
     193           0 :                 _sentence_valid = false;
     194             :             }
     195           0 :             break;
     196             : 
     197           0 :         case 5:
     198             :             // expecting A for data valid
     199           0 :             if (_term[0] != 'A') {
     200           0 :                 _sentence_valid = false;
     201             :             }
     202           0 :             break;
     203             : 
     204             :     }
     205           0 :     return false;
     206             : }
     207             : 
     208             : #endif  // AP_WINDVANE_NMEA_ENABLED