Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED
       4             : 
       5             : #include "SIM_RF_Benewake_TFMiniPlus.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9        2094 : void SITL::Benewake_TFMiniPlus::init()
      10             : {
      11        2094 : }
      12             : 
      13       15518 : int SITL::Benewake_TFMiniPlus::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      14             : {
      15       15518 :     if (data->nmsgs != 1) {
      16           0 :         AP_HAL::panic("Only ever expect a single message from driver");
      17             :     }
      18       15518 :     auto msg = data->msgs[0];
      19       15518 :     if (msg.flags == I2C_M_RD) {
      20             :         // an attempt to retrieve data which was previously requested
      21        7753 :         const auto data_type_requested_for_processing = data_type_requested;
      22        7753 :         data_type_requested = DataTypeRequested::NONE;
      23        7753 :         switch (data_type_requested_for_processing) {
      24           0 :         case DataTypeRequested::NONE:
      25           0 :             AP_HAL::panic("Request to read data when none requested");
      26           2 :         case DataTypeRequested::FW_VERSION: {
      27           2 :             if (msg.len != 7) {
      28           0 :                 AP_HAL::panic("Invalid read length");
      29             :             }
      30           2 :             msg.buf[0] = 0x5A;
      31           2 :             msg.buf[1] = 0x07;
      32           2 :             msg.buf[2] = 0x01;
      33           2 :             msg.buf[3] = 1;
      34           2 :             msg.buf[4] = 2;
      35           2 :             msg.buf[5] = 3;
      36           2 :             msg.buf[6] = crc_sum_of_bytes(msg.buf, 6);
      37           2 :             msg.len = 7;
      38           2 :             return 0;
      39             :         }
      40        7751 :         case DataTypeRequested::READING: {
      41        7751 :             if (msg.len != 11) {
      42           0 :                 AP_HAL::panic("Invalid read length");
      43             :             }
      44        7751 :             if (!running_config.data_output_enabled) {
      45           0 :                 AP_HAL::panic("Request for data before enabling output?!");
      46             :             }
      47        7751 :             float tmp_range = range;
      48        7751 :             if (!running_config.output_format_is_cm) {
      49           0 :                 tmp_range /= 4.5;  // convert to decicubits
      50             :             }
      51        7751 :             const uint16_t strength = 100;
      52        7751 :             uint16_t dist_cm = tmp_range * 100;
      53        7751 :             msg.buf[0] = 0x59;
      54        7751 :             msg.buf[1] = 0x59;
      55        7751 :             msg.buf[2] = dist_cm & 0xff;
      56        7751 :             msg.buf[3] = dist_cm >> 8;
      57        7751 :             msg.buf[4] = strength & 0xff;
      58        7751 :             msg.buf[5] = strength >> 8;
      59        7751 :             msg.buf[6] = 0;  // timestamp
      60        7751 :             msg.buf[7] = 0;  // timestamp
      61        7751 :             msg.buf[8] = 0;  // timestamp
      62        7751 :             msg.buf[9] = 0;  // timestamp
      63        7751 :             msg.buf[10] = crc_sum_of_bytes(msg.buf, 10);
      64        7751 :             msg.len = 11;
      65        7751 :             return 0;
      66             :         }
      67             :         }
      68           0 :         return -1;
      69             :     }
      70             : 
      71             :     // an attempt to retrieve data which was previously requested
      72        7765 :     if (msg.buf[0] != 0x5A) {
      73           0 :         AP_HAL::panic("Unexpected header");
      74             :     }
      75        7765 :     const uint8_t len = msg.buf[1];
      76        7765 :     if (len > msg.len) {
      77           0 :         AP_HAL::panic("Length too long");
      78             :     }
      79        7765 :     if (crc_sum_of_bytes(msg.buf, len-1) != msg.buf[len-1]) {
      80           0 :         AP_HAL::panic("Invalid checksum");
      81             :     }
      82        7765 :     switch (Command(msg.buf[2])) {
      83           2 :     case Command::FW_VERSION:
      84           2 :         data_type_requested = DataTypeRequested::FW_VERSION;
      85           2 :         break;
      86           2 :     case Command::SYSTEM_RESET:
      87           2 :         reset();
      88           2 :         break;
      89           2 :     case Command::OUTPUT_FORMAT_CM:
      90           2 :         new_config.output_format_is_cm = (msg.buf[3] == 1);
      91           2 :         break;
      92           4 :     case Command::ENABLE_DATA_OUTPUT:
      93           4 :         new_config.data_output_enabled = (msg.buf[3] == 1);
      94           4 :         running_config.data_output_enabled = new_config.data_output_enabled;
      95           4 :         break;
      96           2 :     case Command::SET_FRAME_RATE:
      97           2 :         new_config.frame_rate = (msg.buf[3] | msg.buf[4]<<8);
      98           2 :         break;
      99           2 :     case Command::SAVE_SETTINGS:
     100             :         // whatever
     101           2 :         break;
     102        7751 :     case Command::READ_MEASUREMENT:
     103        7751 :         if (msg.buf[3] != 0x07) {
     104           0 :             AP_HAL::panic("Invalid number which I don't know anything about");
     105             :         }
     106        7751 :         data_type_requested = DataTypeRequested::READING;
     107        7751 :         break;
     108             :     }
     109        7765 :     return 0;
     110             : }
     111             : 
     112             : #endif  // AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED