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             :   Simulator for the DTS6012M serial rangefinder
      17             : */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_RF_DTS6012M_ENABLED
      22             : 
      23             : #include "SIM_RF_DTS6012M.h"
      24             : #include <AP_Math/crc.h>
      25             : 
      26             : using namespace SITL;
      27             : 
      28         715 : uint32_t RF_DTS6012M::packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen)
      29             : {
      30         715 :     const uint8_t PACKET_LEN = 23;
      31             : 
      32         715 :     if (buflen < PACKET_LEN) {
      33           0 :         abort();
      34             :     }
      35             : 
      36             :     // sensor sends 0xFFFF when out of range (confirmed on hardware)
      37         715 :     const uint16_t dist_mm = (alt_m * 1000 > 20000) ? 0xFFFF : (uint16_t)(alt_m * 1000);
      38             : 
      39             :     // 7-byte header
      40         715 :     buffer[0] = 0xA5;   // frame header
      41         715 :     buffer[1] = 0x03;   // device ID
      42         715 :     buffer[2] = 0x20;   // device type
      43         715 :     buffer[3] = 0x01;   // command echo (start stream)
      44         715 :     buffer[4] = 0x00;   // reserved
      45         715 :     buffer[5] = 0x00;   // data length high byte (14 = 0x000E, big-endian)
      46         715 :     buffer[6] = 0x0E;   // data length low byte
      47             : 
      48             :     // 14-byte data payload
      49         715 :     buffer[7]  = 0xFF;  // secondary target distance low byte (0xFFFF = invalid)
      50         715 :     buffer[8]  = 0xFF;  // secondary target distance high byte
      51         715 :     buffer[9]  = 0x00;  // secondary target correction low
      52         715 :     buffer[10] = 0x00;  // secondary target correction high
      53         715 :     buffer[11] = 0x00;  // secondary target intensity low
      54         715 :     buffer[12] = 0x00;  // secondary target intensity high
      55         715 :     buffer[13] = dist_mm & 0xFF;  // primary target distance low byte (little-endian, mm)
      56         715 :     buffer[14] = dist_mm >> 8;    // primary target distance high byte
      57         715 :     buffer[15] = 0x00;  // primary target correction low
      58         715 :     buffer[16] = 0x00;  // primary target correction high
      59         715 :     buffer[17] = 0x10;  // primary target intensity low byte (10000 = 0x2710 → 100% quality)
      60         715 :     buffer[18] = 0x27;  // primary target intensity high byte
      61         715 :     buffer[19] = 0x00;  // sunlight base low
      62         715 :     buffer[20] = 0x00;  // sunlight base high
      63             : 
      64             :     // CRC-16/MODBUS over bytes 0–20, stored big-endian (high byte first)
      65         715 :     const uint16_t crc = calc_crc_modbus(buffer, PACKET_LEN - 2);
      66         715 :     buffer[21] = crc >> 8;    // CRC high byte
      67         715 :     buffer[22] = crc & 0xFF;  // CRC low byte
      68             : 
      69         715 :     return PACKET_LEN;
      70             : }
      71             : 
      72             : #endif  // AP_SIM_RF_DTS6012M_ENABLED