Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :   Simulator for the Benewake TFMiniPlus i2c-connected rangefinder
       5             : 
       6             :   Currently we accept writes to the rate register but do not act upon it
       7             :   Output must be enabled before a reading is requested from the device
       8             :   We do understand and act on the unit-type configuration - so if the driver doesn't set the units we do report strange altitudes
       9             : 
      10             : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter --speedup=1
      11             : 
      12             : param set RNGFND1_TYPE 25
      13             : param set RNGFND1_ADDR 0x10
      14             : param ftp
      15             : param set RNGFND1_MAX 10  # ????
      16             : graph DISTANCE_SENSOR[0].current_distance*0.01
      17             : graph GLOBAL_POSITION_INT.relative_alt/1000-DISTANCE_SENSOR[0].current_distance*0.01
      18             : reboot
      19             : 
      20             : arm throttle
      21             : rc 3 1600
      22             : */
      23             : 
      24             : #include "SIM_config.h"
      25             : 
      26             : #if AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED
      27             : 
      28             : #include "SIM_I2CDevice.h"
      29             : 
      30             : namespace SITL {
      31             : 
      32             : class Benewake_TFMiniPlus : public I2CDevice
      33             : {
      34             : public:
      35             : 
      36             :     using I2CDevice::I2CDevice;
      37             : 
      38             :     void init() override;
      39             : 
      40   178759942 :     void update(const class Aircraft &aircraft) override {
      41   178759942 :         const uint32_t now_ms = AP_HAL::millis();
      42   178759942 :         if (last_update_ms - now_ms < 50) {
      43    29855657 :             return;
      44             :         }
      45   148904285 :         last_update_ms = now_ms;
      46   148904285 :         range = aircraft.rangefinder_range();
      47             :     }
      48             : 
      49             : private:
      50             : 
      51             :     enum class DataTypeRequested {
      52             :         NONE = 0,
      53             :         FW_VERSION = 1,
      54             :         READING = 2,
      55             :     };
      56             :     DataTypeRequested data_type_requested;  // defaults to NONE
      57             : 
      58             :     enum class Command {
      59             :         FW_VERSION = 0x01,
      60             :         SYSTEM_RESET = 0x04,
      61             :         OUTPUT_FORMAT_CM = 0x05,
      62             :         ENABLE_DATA_OUTPUT = 0x07,
      63             :         SET_FRAME_RATE = 0x03,
      64             :         SAVE_SETTINGS = 0x11,
      65             :         READ_MEASUREMENT = 0x00,
      66             :     };
      67             : 
      68             :     int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
      69             : 
      70             :     float range;
      71             :     uint32_t last_update_ms;
      72             : 
      73             :     struct Config {
      74             :         uint16_t frame_rate;
      75             :         bool output_format_is_cm;
      76             :         bool data_output_enabled;
      77             :     };
      78             :     Config running_config;
      79             :     Config new_config;
      80             : 
      81           2 :     void reset() { running_config = new_config; }
      82             : };
      83             : 
      84             : } // namespace SITL
      85             : 
      86             : #endif  // AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED