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             : 
      17             : #include "AP_RangeFinder_config.h"
      18             : 
      19             : #if AP_RANGEFINDER_DTS6012M_ENABLED
      20             : 
      21             : #include "AP_RangeFinder_DTS6012M.h"
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_Math/crc.h>
      24             : 
      25             : // DTS6012M protocol constants
      26             : #define DTS6012M_FRAME_HEADER       0xA5
      27             : #define DTS6012M_DEVICE_ID          0x03
      28             : #define DTS6012M_DEVICE_TYPE        0x20
      29             : #define DTS6012M_CMD_START_STREAM   0x01
      30             : #define DTS6012M_HEADER_LEN         7       // header(1) + devid(1) + devtype(1) + cmd(1) + reserved(1) + length(2)
      31             : #define DTS6012M_DATA_LEN           14      // measurement data length
      32             : #define DTS6012M_CRC_LEN            2
      33             : #define DTS6012M_FRAME_LEN          (DTS6012M_HEADER_LEN + DTS6012M_DATA_LEN + DTS6012M_CRC_LEN)  // 23 bytes
      34             : #define DTS6012M_DIST_MAX_MM        20000   // 20m max range
      35             : #define DTS6012M_DIST_INVALID       0xFFFF
      36             : 
      37             : // set to 0 to disable CRC verification if the sensor's CRC proves unreliable
      38             : #ifndef DTS6012M_CRC_ENABLE
      39             : #define DTS6012M_CRC_ENABLE         1
      40             : #endif
      41             : 
      42             : extern const AP_HAL::HAL& hal;
      43             : 
      44             : /*
      45             :   send the start stream command (0x01) to begin periodic measurement output
      46             :   Frame: A5 03 20 01 00 00 00 CRC16_H CRC16_L
      47             : */
      48           1 : void AP_RangeFinder_DTS6012M::send_start_command()
      49             : {
      50             :     static const uint8_t cmd[] = {
      51             :         DTS6012M_FRAME_HEADER,  // 0xA5
      52             :         DTS6012M_DEVICE_ID,     // 0x03
      53             :         DTS6012M_DEVICE_TYPE,   // 0x20
      54             :         DTS6012M_CMD_START_STREAM,  // 0x01
      55             :         0x00,                   // reserved
      56             :         0x00, 0x00              // length = 0 (no data)
      57             :     };
      58             : 
      59             :     // calculate CRC over the command bytes
      60           1 :     const uint16_t crc = calc_crc_modbus(cmd, sizeof(cmd));
      61             : 
      62           1 :     uart->write(cmd, sizeof(cmd));
      63             :     // CRC is sent high byte first per protocol spec
      64           1 :     uart->write(uint8_t(crc >> 8));
      65           1 :     uart->write(uint8_t(crc & 0xFF));
      66           1 : }
      67             : 
      68             : /*
      69             :   search for frame header byte in buffer starting at offset,
      70             :   shifting remaining data to re-sync after line noise
      71             : */
      72           0 : void AP_RangeFinder_DTS6012M::find_signature_in_buffer(uint8_t start)
      73             : {
      74           0 :     uint8_t *buf = (uint8_t *)&frame;
      75           0 :     for (uint8_t i = start; i < linebuf_len; i++) {
      76           0 :         if (buf[i] == DTS6012M_FRAME_HEADER) {
      77           0 :             memmove(&buf[0], &buf[i], linebuf_len - i);
      78           0 :             linebuf_len -= i;
      79           0 :             return;
      80             :         }
      81             :     }
      82           0 :     linebuf_len = 0;
      83             : }
      84             : 
      85             : /*
      86             :   read from the sensor and return distance in meters
      87             :   see frame struct in header for protocol layout
      88             : */
      89        1369 : bool AP_RangeFinder_DTS6012M::get_reading(float &reading_m)
      90             : {
      91        1369 :     if (uart == nullptr) {
      92           0 :         return false;
      93             :     }
      94             : 
      95             :     // keep sending start command until we receive a valid reading,
      96             :     // allowing for slow-startup or late connection of the device
      97        1369 :     if (!got_reading) {
      98           1 :         send_start_command();
      99             :     }
     100             : 
     101             :     // bulk read available bytes into buffer
     102        1369 :     uint8_t *buf = (uint8_t *)&frame;
     103        1369 :     const auto num_read = uart->read(&buf[linebuf_len], sizeof(frame) - linebuf_len);
     104        1369 :     linebuf_len += num_read;
     105             : 
     106        1369 :     if (linebuf_len == 0) {
     107         683 :         return false;
     108             :     }
     109             : 
     110             :     // ensure buffer starts with frame header
     111         686 :     if (frame.header != DTS6012M_FRAME_HEADER) {
     112           0 :         find_signature_in_buffer(1);
     113           0 :         return false;
     114             :     }
     115             : 
     116             :     // wait for a complete frame
     117         686 :     if (linebuf_len < DTS6012M_FRAME_LEN) {
     118           0 :         return false;
     119             :     }
     120             : 
     121             :     // validate header fields
     122         686 :     if (frame.device_id != DTS6012M_DEVICE_ID ||
     123         686 :         frame.device_type != DTS6012M_DEVICE_TYPE ||
     124         686 :         frame.cmd_echo != DTS6012M_CMD_START_STREAM) {
     125           0 :         find_signature_in_buffer(1);
     126           0 :         return false;
     127             :     }
     128             : 
     129             :     // verify length field
     130         686 :     if (be16toh(frame.data_len) != DTS6012M_DATA_LEN) {
     131           0 :         find_signature_in_buffer(1);
     132           0 :         return false;
     133             :     }
     134             : 
     135             :     // verify CRC-16 over header + data (all bytes except last 2)
     136             : #if DTS6012M_CRC_ENABLE
     137         686 :     const uint16_t crc_calc = calc_crc_modbus(buf, DTS6012M_FRAME_LEN - DTS6012M_CRC_LEN);
     138         686 :     if (crc_calc != be16toh(frame.crc)) {
     139           0 :         find_signature_in_buffer(1);
     140           0 :         return false;
     141             :     }
     142             : #endif
     143             : 
     144             :     // extract primary target distance (little-endian, in mm)
     145         686 :     const uint16_t dist_mm = le16toh(frame.primary_distance_mm);
     146             : 
     147             :     // capture primary target intensity for signal quality
     148         686 :     const int32_t intensity = int32_t(le16toh(frame.primary_intensity));
     149             :     // map intensity to 0-100%, clamping at 10000 as practical maximum
     150         686 :     _signal_quality_pct = int8_t(constrain_int32(intensity * 100 / 10000, 0, 100));
     151             : 
     152             :     // frame consumed, reset buffer and discard any stale data so the
     153             :     // next call reads the freshest frame from this high-rate sensor
     154         686 :     linebuf_len = 0;
     155         686 :     uart->discard_input();
     156         686 :     got_reading = true;
     157             : 
     158         686 :     if (dist_mm == DTS6012M_DIST_INVALID || dist_mm > DTS6012M_DIST_MAX_MM) {
     159           0 :         reading_m = max_distance() + 1.0f;
     160           0 :         return true;
     161             :     }
     162             : 
     163         686 :     reading_m = dist_mm * 0.001f;
     164         686 :     return true;
     165             : }
     166             : 
     167             : #endif  // AP_RANGEFINDER_DTS6012M_ENABLED