Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
       4             : 
       5             : #include "SIM_RF_TOFSenseF_I2C.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9             : #include <GCS_MAVLink/GCS.h>
      10             : 
      11   178759942 : void SITL::TOFSenseF_I2C::update(const class Aircraft &aircraft)
      12             : {
      13   178759942 :     const uint32_t now_ms = AP_HAL::millis();
      14   178759942 :     if (last_update_ms - now_ms < 50) {
      15    29855657 :         return;
      16             :     }
      17   148904285 :     last_update_ms = now_ms;
      18             : 
      19   148904285 :     range = aircraft.rangefinder_range();
      20             : }
      21             : 
      22        1387 : int SITL::TOFSenseF_I2C::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      23             : {
      24        1387 :     if (data->nmsgs != 1) {
      25           0 :         abort();
      26             :     }
      27             : 
      28        1387 :     switch (data->msgs[0].flags) {
      29         694 :     case I2C_RDWR:
      30         694 :         return SITL::TOFSenseF_I2C::take_reading(data);
      31         693 :     case I2C_M_RD:
      32         693 :         return SITL::TOFSenseF_I2C::read_data(data);
      33           0 :     default:
      34           0 :         break;
      35             :     }
      36           0 :     abort();
      37             : }
      38             : 
      39         694 : int SITL::TOFSenseF_I2C::take_reading(I2C::i2c_rdwr_ioctl_data *&data)
      40             : {
      41         694 :     auto msg = data->msgs[0];
      42         694 :     if (msg.flags != I2C_RDWR) {
      43           0 :         abort();
      44             :     }
      45             : 
      46         694 :     if (msg.len != 2) {
      47           0 :         abort();
      48             :     }
      49             : 
      50         694 :     if (msg.buf[0] != 0x24) {  // get distance
      51           0 :         abort();
      52             :     }
      53             : 
      54         694 :     if (msg.buf[1] != 0x28) {  // get signal status
      55           0 :         abort();
      56             :     }
      57             : 
      58         694 :     reading_requested = true;
      59             : 
      60         694 :     return 0;
      61             : }
      62             : 
      63         693 : int SITL::TOFSenseF_I2C::read_data(I2C::i2c_rdwr_ioctl_data *&data)
      64             : {
      65         693 :     if (!reading_requested) {
      66           0 :         abort();
      67             :     }
      68             : 
      69         693 :     auto msg = data->msgs[0];
      70             : 
      71         693 :     if (msg.flags != I2C_M_RD) {
      72           0 :         abort();
      73             :     }
      74             : 
      75         693 :     if (msg.len != 8) {
      76           0 :         abort();
      77             :     }
      78             : 
      79         693 :     put_le32_ptr(&msg.buf[0], uint32_t(range*1000));  // m -> mm
      80             : 
      81             :     // 1 here means healthy, 2 here is current signal strength
      82         693 :     const uint32_t signal_strength_and_status = (2U<<16 | 1);
      83         693 :     put_le32_ptr(&msg.buf[4], signal_strength_and_status);
      84             : 
      85         693 :     return 0;
      86             : }
      87             : 
      88             : #endif  // AP_SIM_RF_TOFSENSEF_I2C_ENABLED