AP_WindVane_NMEA.cpp 6.1 KB

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