Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED
       4             : 
       5             : #include "SIM_RF_LightWareI2C_Legacy16Bit.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9        2094 : void SITL::LightWareI2C_Legacy16Bit::init()
      10             : {
      11        2094 :     add_register("RANGE", LightWareI2C_Legacy16BitDevReg::RANGE, I2CRegisters::RegMode::RDONLY);
      12        2094 :     set_register(LightWareI2C_Legacy16BitDevReg::RANGE, (uint16_t)0U);
      13             : 
      14        2094 :     add_register("LOST_SIGNAL_TIMEOUT_READ", LightWareI2C_Legacy16BitDevReg::LOST_SIGNAL_TIMEOUT_READ, I2CRegisters::RegMode::RDONLY);
      15        2094 :     set_register(LightWareI2C_Legacy16BitDevReg::LOST_SIGNAL_TIMEOUT_READ, (uint16_t)0U);
      16             : 
      17        2094 :     add_register("LOST_SIGNAL_TIMEOUT_WRITE", LightWareI2C_Legacy16BitDevReg::LOST_SIGNAL_TIMEOUT_WRITE, I2CRegisters::RegMode::WRONLY);
      18        2094 :     set_register(LightWareI2C_Legacy16BitDevReg::LOST_SIGNAL_TIMEOUT_WRITE, (uint16_t)0U);
      19        2094 : }
      20             : 
      21        1416 : int SITL::LightWareI2C_Legacy16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      22             : {
      23             :     // so this is a bit crap.  The driver first tries to write to a
      24             :     // register on an sf20 device and if nothing interesting happens
      25             :     // tries the legacy driver (which we play with).  We have to
      26             :     // ignore the probe for the sf20:
      27        1416 :     if (data->nmsgs == 1) {
      28           5 :         if (data->msgs[0].len > 6) {
      29           3 :             return -1;
      30             :         }
      31           2 :         if (data->msgs[0].flags != 0) {
      32           0 :             AP_HAL::panic("Unexpected flags");
      33             :         }
      34           2 :         if (data->msgs[1].flags != 0) {
      35           0 :             AP_HAL::panic("Unexpected flags");
      36             :         }
      37           2 :         const uint8_t reg_addr = data->msgs[0].buf[0];
      38           2 :         if (reg_addr == 0x3f) {
      39           1 :             return -1;
      40             :         }
      41             :     }
      42             : 
      43        1412 :     return I2CRegisters_16Bit::rdwr(data);
      44             : }
      45             : 
      46             : #endif  // AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED