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_RangeFinder_NMEA.h"
      17             : 
      18             : #if AP_RANGEFINDER_NMEA_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <ctype.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : // return last value measured by sensor
      26       11465 : bool AP_RangeFinder_NMEA::get_reading(float &reading_m)
      27             : {
      28       11465 :     if (uart == nullptr) {
      29           0 :         return false;
      30             :     }
      31             : 
      32             :     // read any available lines from the lidar
      33       11465 :     float sum = 0.0f;
      34       11465 :     uint16_t count = 0;
      35      103764 :     for (auto i=0; i<8192; i++) {
      36             :         uint8_t c;
      37      103764 :         if (!uart->read(c)) {
      38       11465 :             break;
      39             :         }
      40       92299 :         if (decode(c)) {
      41        1559 :             sum += _distance_m;
      42        1559 :             count++;
      43             :         }
      44             :     }
      45             : 
      46             :     // return false on failure
      47       11465 :     if (count == 0) {
      48        9920 :         return false;
      49             :     }
      50             : 
      51             :     // return average of all measurements
      52        1545 :     reading_m = sum / count;
      53        1545 :     return true;
      54             : }
      55             : 
      56             : // get temperature reading from NMEA MTW sentence
      57       12863 : bool AP_RangeFinder_NMEA::_get_temp(float &temp) const
      58             : {
      59       12863 :     uint32_t now_ms = AP_HAL::millis();
      60       12863 :     if ((_temp_readtime_ms == 0) || ((now_ms - _temp_readtime_ms) > read_timeout_ms())) {
      61           1 :         return false;
      62             :     }
      63       12862 :     temp = _temp;
      64       12862 :     return true;
      65             : }
      66             : 
      67             : // add a single character to the buffer and attempt to decode
      68             : // returns true if a distance was successfully decoded
      69       92299 : bool AP_RangeFinder_NMEA::decode(char c)
      70             : {
      71       92299 :     switch (c) {
      72        4677 :     case ',':
      73             :         // end of a term, add to checksum
      74        4677 :         _checksum ^= c;
      75             :         FALLTHROUGH;
      76       14031 :     case '\r':
      77             :     case '\n':
      78             :     case '*':
      79             :     {
      80       14031 :         if (_sentence_done) {
      81        3118 :             return false;
      82             :         }
      83             : 
      84             :         // null terminate and decode latest term
      85       10913 :         _term[_term_offset] = 0;
      86       10913 :         bool valid_sentence = decode_latest_term();
      87             : 
      88             :         // move onto next term
      89       10913 :         _term_number++;
      90       10913 :         _term_offset = 0;
      91       10913 :         _term_is_checksum = (c == '*');
      92       10913 :         return valid_sentence;
      93             :     }
      94             : 
      95        3118 :     case '$': // sentence begin
      96        3118 :         _sentence_type = SONAR_UNKNOWN;
      97        3118 :         _term_number = 0;
      98        3118 :         _term_offset = 0;
      99        3118 :         _checksum = 0;
     100        3118 :         _term_is_checksum = false;
     101        3118 :         _distance_m = -1.0f;
     102        3118 :         _sentence_done = false;
     103        3118 :         return false;
     104             :     }
     105             : 
     106             :     // ordinary characters are added to term
     107       75150 :     if (_term_offset < sizeof(_term) - 1) {
     108       68914 :         _term[_term_offset++] = c;
     109             :     }
     110       75150 :     if (!_term_is_checksum) {
     111       68914 :         _checksum ^= c;
     112             :     }
     113             : 
     114       75150 :     return false;
     115             : }
     116             : 
     117             : // decode the most recently consumed term
     118             : // returns true if new distance sentence has just passed checksum test and is validated
     119       10913 : bool AP_RangeFinder_NMEA::decode_latest_term()
     120             : {
     121             :     // handle the last term in a message
     122       10913 :     if (_term_is_checksum) {
     123        3118 :         _sentence_done = true;
     124             :         uint8_t checksum;
     125        3118 :         if (!hex_twochars_to_uint8(_term, checksum)) {
     126           0 :             return false;
     127             :         }
     128        3118 :         if (checksum == _checksum) {
     129        3118 :             if ((_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT || _sentence_type == SONAR_HDED) && !is_negative(_distance_m)) {
     130             :                 // return true if distance is valid
     131        1559 :                 return true;
     132             :             }
     133        1559 :             if (_sentence_type == SONAR_MTW) {
     134        1559 :                 _temp = _temp_unvalidated;
     135        1559 :                 _temp_readtime_ms = AP_HAL::millis();
     136             :                 // return false because this is not a distance
     137             :                 // temperature is accessed via separate accessor
     138        1559 :                 return false;
     139             :             }
     140             :         }
     141           0 :         return false;
     142             :     }
     143             : 
     144             :     // the first term determines the sentence type
     145        7795 :     if (_term_number == 0) {
     146             :         // the first two letters of the NMEA term are the talker ID.
     147             :         // we accept any two characters here.
     148        3118 :         if (_term[0] < 'A' || _term[0] > 'Z' ||
     149        3118 :             _term[1] < 'A' || _term[1] > 'Z') {
     150           0 :             _sentence_type = SONAR_UNKNOWN;
     151           0 :             return false;
     152             :         }
     153        3118 :         const char *term_type = &_term[2];
     154        3118 :         if (strcmp(term_type, "DBT") == 0) {
     155           0 :             _sentence_type = SONAR_DBT;
     156        3118 :         } else if (strcmp(term_type, "DPT") == 0) {
     157        1559 :             _sentence_type = SONAR_DPT;
     158        1559 :         } else if (strcmp(term_type, "MTW") == 0) {
     159        1559 :             _sentence_type = SONAR_MTW;
     160           0 :         } else if (strcmp(term_type, "ED") == 0) {
     161           0 :             _sentence_type = SONAR_HDED;
     162             :         } else {
     163           0 :             _sentence_type = SONAR_UNKNOWN;
     164             :         }
     165        3118 :         return false;
     166             :     }
     167             : 
     168        4677 :     if (_sentence_type == SONAR_DBT) {
     169             :         // parse DBT messages
     170           0 :         if (_term_number == 3) {
     171           0 :             _distance_m = strtof(_term, NULL);
     172             :         }
     173        4677 :     } else if (_sentence_type == SONAR_DPT) {
     174             :         // parse DPT messages
     175        3118 :         if (_term_number == 1) {
     176        1559 :             _distance_m = strtof(_term, NULL);
     177             :         }
     178        1559 :     } else if (_sentence_type == SONAR_MTW) {
     179             :         // parse MTW (mean water temperature) messages
     180        1559 :         if (_term_number == 1) {
     181        1559 :             _temp_unvalidated = strtof(_term, NULL);
     182             :         }
     183           0 :     } else if (_sentence_type == SONAR_HDED) {
     184             :         // parse HDED (Hondex custom message)
     185           0 :         if (_term_number == 4) {
     186           0 :             _distance_m = strtof(_term, NULL);
     187             :         }
     188             :     }
     189             : 
     190        4677 :     return false;
     191             : }
     192             : 
     193             : #endif  // AP_RANGEFINDER_NMEA_ENABLED