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 RDS02UF rangefinder
      17             : */
      18             : 
      19             : #include "SIM_RF_RDS02UF.h"
      20             : 
      21             : #include <stdio.h>
      22             : #include <AP_Math/crc.h>
      23             : 
      24             : using namespace SITL;
      25             : 
      26         355 : uint32_t RF_RDS02UF::packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen)
      27             : {
      28         355 :     const uint16_t alt_cm = alt_m * 100;
      29             : 
      30         355 :     const uint16_t fc_code = 0x3ff;  // NFI what this means
      31         355 :     const uint16_t data_fc = 0x70c;  // NFI what this means
      32             : 
      33             :     // bodgy fixed-length response to keep things simple:
      34             :     union response_t {
      35             :         struct {
      36             :             uint8_t header1;
      37             :             uint8_t header2;
      38             :             uint8_t address;
      39             :             uint8_t error_code;
      40             :             uint8_t fc_code_l;
      41             :             uint8_t fc_code_h;
      42             :             uint8_t length_l;
      43             :             uint8_t length_h;
      44             :             uint8_t data0;  // used
      45             :             uint8_t data1;  // used
      46             :             uint8_t data2;
      47             :             uint8_t data3;
      48             :             uint8_t data4;
      49             :             uint8_t data5;  // distance-low
      50             :             uint8_t data6;  // distance-high
      51             :             uint8_t data7;
      52             :             uint8_t data8;
      53             :             uint8_t data9;
      54             :             uint8_t crc8;
      55             :             uint8_t footer1;
      56             :             uint8_t footer2;
      57             :         };
      58             :         uint8_t buffer[21];
      59         355 :     } response{};
      60             : 
      61         355 :     response.header1 = 0x55;
      62         355 :     response.header2 = 0x55;
      63         355 :     response.fc_code_l = fc_code & 0xff;
      64         355 :     response.fc_code_h = fc_code >> 8;
      65         355 :     response.length_l = 10;
      66         355 :     response.length_h = 0;
      67         355 :     response.data0 = data_fc & 0xff;
      68         355 :     response.data1 = data_fc >> 8;
      69         355 :     response.data5 = alt_cm  & 0xff;
      70         355 :     response.data6 = alt_cm >> 8;
      71         355 :     response.crc8 = crc8_rds02uf(&response.buffer[2], 16);
      72         355 :     response.footer1 = 0xAA;
      73         355 :     response.footer2 = 0xAA;
      74             : 
      75         355 :     if (buflen < ARRAY_SIZE(response.buffer)) {
      76           0 :         AP_HAL::panic("Too short a buffer");
      77             :     }
      78             : 
      79         355 :     memcpy(buffer, response.buffer, ARRAY_SIZE(response.buffer));
      80             : 
      81         355 :     return ARRAY_SIZE(response.buffer);
      82             : }