Line data    Source code

       1             : #include "SIM_I2CDevice.h"
       2             : 
       3             : namespace SITL {
       4             : 
       5             : class InvensenseV3DevReg : public I2CRegEnum {
       6             : public:
       7             :     static const uint8_t WHOAMI        = 0x75;
       8             :     // static const uint8_t INT_CONFIG    = 0x14;
       9             :     static const uint8_t FIFO_CONFIG   = 0x16;
      10             :     static const uint8_t PWR_MGMT0     = 0x4e;
      11             :     static const uint8_t GYRO_CONFIG0  = 0x4f;
      12             :     static const uint8_t ACCEL_CONFIG0 = 0x50;
      13             :     static const uint8_t FIFO_CONFIG1  = 0x5f;
      14             :     // static const uint8_t FIFO_CONFIG2  = 0x60;
      15             :     // static const uint8_t FIFO_CONFIG3  = 0x61;
      16             :     // static const uint8_t INT_SOURCE0   = 0x65;
      17             :     static const uint8_t SIGNAL_PATH_RESET = 0x4b;
      18             :     static const uint8_t INTF_CONFIG0  = 0x4c;
      19             :     static const uint8_t FIFO_COUNTH   = 0x2e;
      20             :     static const uint8_t FIFO_DATA     = 0x30;
      21             :     // static const uint8_t BANK_SEL      = 0x76;
      22             : };
      23             : 
      24             : class InvensenseV3 : public I2CDevice, protected I2CRegisters_8Bit
      25             : {
      26             : public:
      27        2094 :     void init() override {
      28        2094 :         add_register("WHOAMI", InvensenseV3DevReg::WHOAMI, I2CRegisters::RegMode::RDONLY);
      29        2094 :         add_register("FIFO_CONFIG", InvensenseV3DevReg::FIFO_CONFIG, I2CRegisters::RegMode::RDWR);
      30        2094 :         add_register("FIFO_CONFIG1", InvensenseV3DevReg::FIFO_CONFIG1, I2CRegisters::RegMode::RDWR);
      31        2094 :         add_register("INTF_CONFIG0", InvensenseV3DevReg::INTF_CONFIG0, I2CRegisters::RegMode::RDWR);
      32        2094 :         add_register("SIGNAL_PATH_RESET", InvensenseV3DevReg::SIGNAL_PATH_RESET, I2CRegisters::RegMode::RDWR);
      33        2094 :         add_register("PWR_MGMT0", InvensenseV3DevReg::PWR_MGMT0, I2CRegisters::RegMode::RDWR);
      34        2094 :         add_register("GYRO_CONFIG0", InvensenseV3DevReg::GYRO_CONFIG0, I2CRegisters::RegMode::RDWR);
      35        2094 :         add_register("ACCDEL_CONFIG0", InvensenseV3DevReg::ACCEL_CONFIG0, I2CRegisters::RegMode::RDWR);
      36             : 
      37             :         // sample count!
      38        2094 :         add_block("FIFO_COUNTH", InvensenseV3DevReg::FIFO_COUNTH, 2, I2CRegisters::RegMode::RDONLY);
      39             : 
      40        2094 :         add_fifo("FIFO_DATA", InvensenseV3DevReg::FIFO_DATA, I2CRegisters::RegMode::RDONLY);
      41        2094 :     }
      42             : 
      43             :     void update(const class Aircraft &aircraft) override;
      44             : 
      45             :     int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
      46             : 
      47             :     virtual float accel_scale() const = 0;
      48   536279826 :     float gyro_scale() const { return (0.0174532f / 16.4f); }
      49             : 
      50             : private:
      51             : 
      52             :     // sets the FIFO_COUNTH block based on the number of samples in FIFO_DATA
      53             :     void update_sample_count();
      54             : 
      55             :     void assert_register_values();
      56             : 
      57             :     void add_fifo(const char *name, uint8_t reg, I2CRegisters::RegMode mode);
      58             :     bool write_to_fifo(uint8_t fifo, uint8_t *value, uint8_t valuelen);
      59             : 
      60             :     int rdwr_fifo(I2C::i2c_rdwr_ioctl_data *&data);
      61             :     static const uint8_t fifo_len = 64;
      62             :     const char *fifoname[256];
      63             :     Bitmask<256> writable_fifos;
      64             :     Bitmask<256> readable_fifos;
      65             : 
      66             :     // 256 pointers-to-malloced-values:
      67             :     char *values[256];
      68             :     uint8_t value_lengths[256];
      69             : 
      70             :     // swiped from the driver:
      71             :     struct PACKED FIFOData {
      72             :         uint8_t header;
      73             :         int16_t accel[3];
      74             :         int16_t gyro[3];
      75             :         int8_t temperature;
      76             :         uint16_t timestamp;
      77             :     };
      78             : 
      79             :     // block support
      80             :     // for things that are larger than a register....
      81             :     int rdwr_block(I2C::i2c_rdwr_ioctl_data *&data);
      82             :     void add_block(const char *name, uint8_t addr, uint8_t len, I2CRegisters::RegMode mode);
      83             :     void set_block(uint8_t addr, uint8_t *value, uint8_t valuelen);
      84             : 
      85             :     const char *blockname[256];
      86             :     Bitmask<256> writable_blocks;
      87             :     Bitmask<256> readable_blocks;
      88             : 
      89             :     // 256 pointers-to-malloced-values:
      90             :     char *block_values[256];
      91             :     uint8_t block_value_lengths[256];
      92             : };
      93             : 
      94             : } // namespace SITL