Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :   Simulator for the Benewake TFS20L rangefinder
       5             : 
       6             : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter --speedup=1
       7             : 
       8             : param set RNGFND1_TYPE 45  # TFS20L
       9             : param set RNGFND1_ADDR 0x10
      10             : graph RANGEFINDER.distance
      11             : graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance
      12             : reboot
      13             : 
      14             : arm throttle
      15             : rc 3 1600
      16             : */
      17             : 
      18             : #include "SIM_config.h"
      19             : 
      20             : #if AP_SIM_TFS20L_ENABLED
      21             : 
      22             : #include <cstdint>
      23             : #include "SIM_I2CDevice.h"
      24             : 
      25             : namespace SITL {
      26             : 
      27             : class TFS20L : public I2CDevice {
      28             : public:
      29        2094 :     void init() override {
      30        2094 :         rangefinder_range = 0;
      31        2094 :         strength = MIN_STRENGTH;
      32        2094 :         reg = 0;
      33        2094 :         last_update_ms = 0;
      34        2094 :     }
      35             : 
      36             :     void update(const class Aircraft &aircraft) override;
      37             : 
      38             :     // Used for testing
      39             :     void update(float distance_m) {
      40             :         rangefinder_range = distance_m;
      41             :         strength = MIN_STRENGTH;
      42             :     }
      43             : 
      44             : #ifdef SITL_I2C_TEST
      45             :     // Read distance directly for testing
      46             :     void read_distance(uint8_t *buf) {
      47             :         uint16_t dist_cm = rangefinder_range * 100.0f;
      48             :         buf[0] = dist_cm & 0xFF;         // Low byte
      49             :         buf[1] = (dist_cm >> 8) & 0xFF;  // High byte
      50             :     }
      51             : #endif
      52             : 
      53             :     int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
      54             : 
      55             : private:
      56             :     float rangefinder_range;         // Range in meters
      57             :     uint16_t strength{MIN_STRENGTH}; // Signal strength
      58             :     uint8_t reg{0};                  // Current register for I2C operations
      59             :     uint32_t last_update_ms;         // Last time sensor was updated
      60             :     static const uint16_t MIN_STRENGTH = 100;
      61             : };
      62             : 
      63             : } // namespace SITL
      64             : 
      65             : #endif  // AP_SIM_TFS20L_ENABLED