Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_TERARANGERI2C_ENABLED
       4             : 
       5             : #include "SIM_TeraRangerI2C.h"
       6             : 
       7             : using namespace SITL;
       8             : 
       9       14294 : int TeraRangerI2C::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      10             : {
      11       14294 :     if (data->nmsgs == 2) {
      12             :         // read/write message
      13           1 :         const uint8_t reg_base_addr = data->msgs[0].buf[0];
      14           1 :         if (reg_base_addr == 0) {
      15           0 :             AP_HAL::panic("Should not get combed read/write for TRIGGER_READING");
      16             :         }
      17             :     }
      18             : 
      19       14294 :     if (data->msgs[0].flags == I2C_M_RD) {
      20        7146 :         if (reading_start_us != 0) {
      21        7146 :             if (data->nmsgs != 1) {
      22           0 :                 AP_HAL::panic("Unexpected number of i2c messages");
      23             :             }
      24        7146 :             const auto &msg = data->msgs[0];
      25        7146 :             if (reading_start_us == 0) {
      26           0 :                 AP_HAL::panic("Attempt to read sample without requesting it");
      27             :             }
      28        7146 :             const uint32_t now_us = MAX(AP_HAL::micros(), 1U);
      29        7146 :             if (now_us - reading_start_us < 500) {
      30           0 :                 AP_HAL::panic("Attempt to read sensor before data would be ready");
      31             :             }
      32        7146 :             const uint16_t reading = MIN(rangefinder_range*1000, 65535);
      33        7146 :             msg.buf[0] = reading >> 8;
      34        7146 :             msg.buf[1] = reading & 0xff;
      35        7146 :             msg.buf[2] = crc_crc8(msg.buf, 2);
      36        7146 :             reading_start_us = 0;
      37        7146 :             return 0;
      38             :         }
      39        7148 :     } else if (data->msgs[0].flags == 0) {  // write request
      40        7148 :         const uint8_t reg_base_addr = data->msgs[0].buf[0];
      41        7148 :         if (reg_base_addr == 0) {
      42        7147 :             if (reading_start_us != 0) {
      43           0 :                 AP_HAL::panic("Requesting sample without reading previous one?");
      44             :             }
      45        7147 :             reading_start_us = MAX(AP_HAL::micros(), 1U);
      46        7147 :             return 0;
      47             :         }
      48             :     } else {
      49           0 :         AP_HAL::panic("Bad flags in i2c transfer %02x", data->msgs[0].flags);
      50             :     }
      51             : 
      52           1 :     return I2CRegisters_8Bit::rdwr(data);
      53             : }
      54             : 
      55             : #endif  // AP_SIM_TERARANGERI2C_ENABLED