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             :  *   NMEA Sensor driver for VHW ans MTW messages over Serial
      17             :  *   https://gpsd.gitlab.io/gpsd/NMEA.html#_vhw_water_speed_and_heading
      18             :  *   https://gpsd.gitlab.io/gpsd/NMEA.html#_mtw_mean_temperature_of_water
      19             :  */
      20             : 
      21             : #include "AP_Airspeed_NMEA.h"
      22             : 
      23             : #if AP_AIRSPEED_NMEA_ENABLED
      24             : 
      25             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      26             : #if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
      27             : 
      28             : #include "AP_Airspeed.h"
      29             : #include <AP_SerialManager/AP_SerialManager.h>
      30             : 
      31             : #define TIMEOUT_MS 2000
      32             : 
      33           0 : bool AP_Airspeed_NMEA::init()
      34             : {
      35           0 :     const AP_SerialManager& serial_manager = AP::serialmanager();
      36             : 
      37           0 :     _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_AirSpeed, 0);
      38           0 :     if (_uart == nullptr) {
      39           0 :         return false;
      40             :     }
      41             : 
      42           0 :     set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,0,0,0));
      43             : 
      44           0 :     _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_AirSpeed, 0));
      45             : 
      46             :     // make sure this sensor cannot be used in the EKF
      47           0 :     set_use(0);
      48             : 
      49             :     // must set use zero offset to pass offset check for health
      50           0 :     set_use_zero_offset();
      51             : 
      52           0 :     return true;
      53             : }
      54             : 
      55             : // read the from the sensor
      56           0 : bool AP_Airspeed_NMEA::get_airspeed(float &airspeed)
      57             : {
      58           0 :     if (_uart == nullptr) {
      59           0 :         return false;
      60             :     }
      61             : 
      62           0 :     uint32_t now = AP_HAL::millis();
      63             : 
      64             :     // read any available lines from the sensor
      65           0 :     float sum = 0.0f;
      66           0 :     uint16_t count = 0;
      67           0 :     int16_t nbytes = _uart->available();
      68           0 :     while (nbytes-- > 0) {
      69           0 :         int16_t c = _uart->read();
      70           0 :         if (c==-1) {
      71           0 :             return false;
      72             :         }
      73           0 :         if (decode(char(c))) {
      74           0 :             _last_update_ms = now;
      75           0 :             if (_sentence_type == TYPE_VHW) {
      76           0 :                 sum += _speed;
      77           0 :                 count++;
      78             :             } else {
      79           0 :                 _temp_sum += _temp;
      80           0 :                 _temp_count++;
      81             :             }
      82             :         }
      83             :     }
      84             : 
      85           0 :     if (count == 0) {
      86             :         // Cant return false because updates are too slow, return previous reading
      87             :         // Could return false after some timeout, however testing shows that the DST800 just stops sending the message at zero speed
      88           0 :         airspeed = _last_speed;
      89             :     } else {
      90             :         // return average of all measurements
      91           0 :         airspeed = sum / count;
      92           0 :         _last_speed = airspeed;
      93             :     }
      94             : 
      95           0 :     return (now - _last_update_ms) < TIMEOUT_MS;
      96             : }
      97             : 
      98             : // return the current temperature in degrees C
      99             : // the main update is done in the get_pressure function
     100             : // this just reports the value
     101           0 : bool AP_Airspeed_NMEA::get_temperature(float &temperature)
     102             : {
     103           0 :     if (_uart == nullptr) {
     104           0 :         return false;
     105             :     }
     106             : 
     107           0 :     if (_temp_count == 0) {
     108           0 :         temperature = _last_temp;
     109             :     } else {
     110             :         // return average of all measurements
     111           0 :         temperature = _temp_sum / _temp_count;
     112           0 :         _last_temp = temperature;
     113           0 :         _temp_count = 0;
     114           0 :         _temp_sum = 0;
     115             :     }
     116             : 
     117           0 :     return true;
     118             : }
     119             : 
     120             : 
     121             : // add a single character to the buffer and attempt to decode
     122             : // returns true if a complete sentence was successfully decoded
     123           0 : bool AP_Airspeed_NMEA::decode(char c)
     124             : {
     125           0 :     switch (c) {
     126           0 :     case ',':
     127             :         // end of a term, add to checksum
     128           0 :         _checksum ^= c;
     129             :         FALLTHROUGH;
     130           0 :     case '\r':
     131             :     case '\n':
     132             :     case '*':
     133             :     {
     134           0 :         if (!_sentence_done && _sentence_valid) {
     135             :             // null terminate and decode latest term
     136           0 :             _term[_term_offset] = 0;
     137           0 :             bool valid_sentence = decode_latest_term();
     138             : 
     139             :             // move onto next term
     140           0 :             _term_number++;
     141           0 :             _term_offset = 0;
     142           0 :             _term_is_checksum = (c == '*');
     143           0 :             return valid_sentence;
     144             :         }
     145           0 :         return false;
     146             :     }
     147             : 
     148           0 :     case '$': // sentence begin
     149           0 :         _term_number = 0;
     150           0 :         _term_offset = 0;
     151           0 :         _checksum = 0;
     152           0 :         _term_is_checksum = false;
     153           0 :         _sentence_done = false;
     154           0 :         _sentence_valid = true;
     155           0 :         return false;
     156             :     }
     157             : 
     158             :     // ordinary characters are added to term
     159           0 :     if (_term_offset < sizeof(_term) - 1) {
     160           0 :         _term[_term_offset++] = c;
     161             :     }
     162           0 :     if (!_term_is_checksum) {
     163           0 :         _checksum ^= c;
     164             :     }
     165             : 
     166           0 :     return false;
     167             : }
     168             : 
     169             : // decode the most recently consumed term
     170             : // returns true if new sentence has just passed checksum test and is validated
     171           0 : bool AP_Airspeed_NMEA::decode_latest_term()
     172             : {
     173             :     // handle the last term in a message
     174           0 :     if (_term_is_checksum) {
     175           0 :         _sentence_done = true;
     176             :         uint8_t checksum;
     177           0 :         if (!hex_twochars_to_uint8(_term, checksum)) {
     178           0 :             return false;
     179             :         }
     180           0 :         return checksum == _checksum;
     181             :     }
     182             : 
     183             :     // the first term determines the sentence type
     184           0 :     if (_term_number == 0) {
     185             :         // the first two letters of the NMEA term are the talker ID.
     186             :         // we accept any two characters here.
     187             :         // actually expecting YX for MTW and VW for VHW
     188           0 :         if (_term[0] < 'A' || _term[0] > 'Z' ||
     189           0 :             _term[1] < 'A' || _term[1] > 'Z') {
     190           0 :             return false;
     191             :         }
     192           0 :         const char *term_type = &_term[2];
     193           0 :         if (strcmp(term_type, "MTW") == 0) {
     194           0 :             _sentence_type = TPYE_MTW;
     195           0 :         } else if (strcmp(term_type, "VHW") == 0) {
     196           0 :             _sentence_type = TYPE_VHW;
     197             :         } else {
     198           0 :             _sentence_valid = false;
     199             :         }
     200           0 :         return false;
     201             :     }
     202             : 
     203           0 :     if (_sentence_type == TPYE_MTW) {
     204             :         // parse MTW messages
     205           0 :         if (_term_number == 1) {
     206           0 :             _temp = strtof(_term, NULL);
     207             :         }
     208           0 :     } else if (_sentence_type == TYPE_VHW) {
     209             :         // parse VHW messages
     210           0 :         if (_term_number == 7) {
     211           0 :             _speed = strtof(_term, NULL) * KM_PER_HOUR_TO_M_PER_SEC;
     212             :         }
     213             :     }
     214             : 
     215           0 :     return false;
     216             : }
     217             : 
     218             : #endif  // APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) 
     219             : 
     220             : #endif  // AP_AIRSPEED_NMEA_ENABLED