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_USD1_Serial.h"
      17             : 
      18             : #if AP_RANGEFINDER_USD1_SERIAL_ENABLED
      19             : 
      20             : #include <ctype.h>
      21             : 
      22             : #define USD1_HDR 254   // Header Byte from USD1_Serial (0xFE)
      23             : #define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
      24             : 
      25             : /*
      26             :    detect USD1_Serial Firmware Version
      27             : */
      28        2710 : bool AP_RangeFinder_USD1_Serial::detect_version(void)
      29             : {
      30        2710 :     if (_version_known) {
      31             :         // return true if we've already detected the USD1_Serial version
      32        2708 :         return true;
      33           2 :     } else if (uart == nullptr) {
      34           0 :         return false;
      35             :     }
      36             : 
      37           2 :     bool hdr_found = false;
      38           2 :     uint8_t byte1 = 0;
      39           2 :     uint8_t count = 0;
      40             : 
      41             :     // read any available data from USD1_Serial
      42           6 :     for (auto i=0; i<8192; i++) {
      43             :         uint8_t c;
      44           6 :         if (!uart->read(c)) {
      45           0 :             break;
      46             :         }
      47           6 :         if (((c == USD1_HDR_V0) || (c == USD1_HDR)) && !hdr_found) {
      48           2 :             byte1 = c;
      49           2 :             hdr_found = true;
      50           2 :             count++;
      51           4 :         } else if (hdr_found) {
      52           4 :             if (byte1 == USD1_HDR_V0) {
      53           3 :                 if (++count < 4) {
      54             :                     /* need to collect 4 bytes to check for recurring
      55             :                      * header byte in the old 3-byte data format
      56             :                      */
      57           2 :                     continue;
      58             :                 } else {
      59           1 :                     if (c == byte1) {
      60             :                         // if header byte is recurring, set USD1_Serial Version
      61           1 :                         _version = 0;
      62           1 :                         _header = USD1_HDR_V0;
      63           1 :                         _version_known = true;
      64           2 :                         return true;
      65             :                     } else {
      66             :                         /* if V0 header byte didn't occur again on 4th byte,
      67             :                          * start the search again for a header byte
      68             :                          */
      69           0 :                         count = 0;
      70           0 :                         byte1 = 0;
      71           0 :                         hdr_found = false;
      72             :                     }
      73             :                 }
      74             :             } else {
      75           1 :                 if ((c & 0x80) || (c == USD1_HDR_V0)) {
      76             :                     /* Though unlikely, it is possible we could find USD1_HDR
      77             :                      * in a data byte from the old 3-byte format. In this case,
      78             :                      * either the next byte is another data byte (which by default
      79             :                      * is of the form 0x1xxxxxxx), or the next byte is the old 
      80             :                      * header byte (USD1_HDR_V0). In this case, start the search
      81             :                      * again for a header byte.
      82             :                      */
      83           0 :                     count = 0;
      84           0 :                     byte1 = 0;
      85           0 :                     hdr_found = false;
      86             :                 } else {
      87             :                     /* if this second byte passes the above if statement, this byte
      88             :                      * is the version number
      89             :                      */
      90           1 :                     _version = c;
      91           1 :                     _header = USD1_HDR;
      92           1 :                     _version_known = true;
      93           1 :                     return true;
      94             :                 }
      95             :             }
      96             :         }
      97             :     }
      98             : 
      99             :     /* return false if we've gone through all available data
     100             :      * and haven't detected a USD1_Serial firmware version
     101             :      */
     102           0 :     return false;
     103             : }
     104             : 
     105             : 
     106             : // read - return last value measured by sensor
     107        2710 : bool AP_RangeFinder_USD1_Serial::get_reading(float &reading_m)
     108             : {
     109        2710 :     if (uart == nullptr) {
     110           0 :         return false;
     111             :     }
     112             : 
     113             :     
     114        2710 :     if (!detect_version()) {
     115             :         // return false if USD1_Serial version check failed
     116           0 :         return false;
     117             :     }
     118             : 
     119             :     // read any available lines from the USD1_Serial
     120        2710 :     float sum = 0;
     121        2710 :     uint16_t count = 0;
     122        2710 :     bool hdr_found = false;
     123             : 
     124        6850 :     for (auto i=0; i<8192; i++) {
     125             :         uint8_t c;
     126        6850 :         if (!uart->read(c)) {
     127        2710 :             break;
     128             :         }
     129        4140 :         if ((c == _header) && !hdr_found) {
     130             :             // located header byte
     131         689 :             _linebuf_len = 0;
     132         689 :             hdr_found   = true;
     133             :         }
     134             :         // decode index information
     135        4140 :         if (hdr_found) {
     136        4134 :             _linebuf[_linebuf_len++] = c;
     137             : 
     138        4134 :             if ((_linebuf_len < (sizeof(_linebuf)/sizeof(_linebuf[0]))) ||
     139         689 :                 (_version == 0 && _linebuf_len < 3)) {
     140             :                 /* don't process _linebuf until we've collected six bytes of data
     141             :                  * (or 3 bytes for Version 0 firmware)
     142             :                  */
     143        3445 :                 continue;
     144             :             } else {
     145         689 :                 if (_version == 0 && _header != USD1_HDR) {
     146             :                     // parse data for Firmware Version #0
     147         344 :                     sum += (_linebuf[2]&0x7F)*128 + (_linebuf[1]&0x7F);
     148         344 :                     count++;
     149             :                 } else {
     150             :                     // evaluate checksum
     151         345 :                     if (((_linebuf[1] + _linebuf[2] + _linebuf[3] + _linebuf[4]) & 0xFF) == _linebuf[5]) {
     152             :                         // if checksum passed, parse data for Firmware Version #1
     153         345 :                         sum += _linebuf[3]*256 + _linebuf[2];
     154         345 :                         count++;
     155             :                     }
     156             :                 }
     157             : 
     158         689 :                 hdr_found = false;
     159         689 :                 _linebuf_len = 0;
     160             :             }
     161             :         }
     162             :     }
     163             : 
     164        2710 :     if (count == 0) {
     165        2029 :         return false;
     166             :     }
     167             : 
     168         681 :     reading_m = (sum * 0.01) / count;
     169             : 
     170         681 :     if (_version == 0 && _header != USD1_HDR) {
     171         338 :         reading_m *= 2.5;
     172             :     }
     173             : 
     174         681 :     return true;
     175             : }
     176             : 
     177             : #endif  // AP_RANGEFINDER_USD1_SERIAL_ENABLED