Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :   Simulator for the legacy-protocol LightWareI2C rangefinder
       5             : 
       6             : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter --speedup=1
       7             : 
       8             : param set RNGFND1_TYPE 7
       9             : param set RNGFND1_ADDR 0x66
      10             : param set RNGFND1_MAX 30
      11             : graph DISTANCE_SENSOR[0].current_distance*0.01
      12             : graph GLOBAL_POSITION_INT.relative_alt/1000-DISTANCE_SENSOR[0].current_distance*0.01
      13             : reboot
      14             : 
      15             : arm throttle
      16             : rc 3 1600
      17             : */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED
      22             : 
      23             : #include "SIM_I2CDevice.h"
      24             : 
      25             : namespace SITL {
      26             : 
      27             : class LightWareI2C_Legacy16BitDevReg : public I2CRegEnum {
      28             : public:
      29             :     static const uint8_t RANGE                               =  0;
      30             :     static const uint8_t LOST_SIGNAL_TIMEOUT_READ            = 22;
      31             :     static const uint8_t LOST_SIGNAL_TIMEOUT_WRITE           = 23;
      32             : };
      33             : 
      34             : class LightWareI2C_Legacy16Bit : public I2CDevice, private I2CRegisters_16Bit
      35             : {
      36             : public:
      37             : 
      38        2145 :     LightWareI2C_Legacy16Bit() :
      39             :         I2CDevice(),
      40        2145 :         I2CRegisters_16Bit()
      41        2145 :         { }
      42             : 
      43             :     void init() override;
      44             : 
      45   178759942 :     void update(const class Aircraft &aircraft) override {
      46   178759942 :         const uint32_t now_ms = AP_HAL::millis();
      47   178759942 :         if (last_update_ms - now_ms > 50) {
      48   178755924 :             const float range = aircraft.rangefinder_range();
      49   178755924 :             if (range > 30) {
      50    55278870 :                 set_register(LightWareI2C_Legacy16BitDevReg::RANGE, htobe16(uint16_t(65436)));  // firmware can send this value for out-of-range
      51             :             } else {
      52   123477054 :                 set_register(LightWareI2C_Legacy16BitDevReg::RANGE, htobe16(uint16_t(range*100)));
      53             :             }
      54             :         }
      55   178759942 :     }
      56             : 
      57             : private:
      58             : 
      59             :     int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
      60             : 
      61             :     uint32_t last_update_ms;
      62             : };
      63             : 
      64             : } // namespace SITL
      65             : 
      66             : #endif  // AP_SIM_RF_LIGHTWAREI2C_LEGACY16BIT_ENABLED