You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
94 lines
3.4 KiB
94 lines
3.4 KiB
#include "SIM_I2CDevice.h" |
|
|
|
namespace SITL { |
|
|
|
class InvensenseV3DevReg : public I2CRegEnum { |
|
public: |
|
static const uint8_t WHOAMI = 0x75; |
|
// static const uint8_t INT_CONFIG = 0x14; |
|
static const uint8_t FIFO_CONFIG = 0x16; |
|
static const uint8_t PWR_MGMT0 = 0x4e; |
|
static const uint8_t GYRO_CONFIG0 = 0x4f; |
|
static const uint8_t ACCEL_CONFIG0 = 0x50; |
|
static const uint8_t FIFO_CONFIG1 = 0x5f; |
|
// static const uint8_t FIFO_CONFIG2 = 0x60; |
|
// static const uint8_t FIFO_CONFIG3 = 0x61; |
|
// static const uint8_t INT_SOURCE0 = 0x65; |
|
static const uint8_t SIGNAL_PATH_RESET = 0x4b; |
|
static const uint8_t INTF_CONFIG0 = 0x4c; |
|
static const uint8_t FIFO_COUNTH = 0x2e; |
|
static const uint8_t FIFO_DATA = 0x30; |
|
// static const uint8_t BANK_SEL = 0x76; |
|
}; |
|
|
|
class InvensenseV3 : public I2CDevice, protected I2CRegisters_8Bit |
|
{ |
|
public: |
|
void init() override { |
|
add_register("WHOAMI", InvensenseV3DevReg::WHOAMI, I2CRegisters::RegMode::RDONLY); |
|
add_register("FIFO_CONFIG", InvensenseV3DevReg::FIFO_CONFIG, I2CRegisters::RegMode::RDWR); |
|
add_register("FIFO_CONFIG1", InvensenseV3DevReg::FIFO_CONFIG1, I2CRegisters::RegMode::RDWR); |
|
add_register("INTF_CONFIG0", InvensenseV3DevReg::INTF_CONFIG0, I2CRegisters::RegMode::RDWR); |
|
add_register("SIGNAL_PATH_RESET", InvensenseV3DevReg::SIGNAL_PATH_RESET, I2CRegisters::RegMode::RDWR); |
|
add_register("PWR_MGMT0", InvensenseV3DevReg::PWR_MGMT0, I2CRegisters::RegMode::RDWR); |
|
add_register("GYRO_CONFIG0", InvensenseV3DevReg::GYRO_CONFIG0, I2CRegisters::RegMode::RDWR); |
|
add_register("ACCDEL_CONFIG0", InvensenseV3DevReg::ACCEL_CONFIG0, I2CRegisters::RegMode::RDWR); |
|
|
|
// sample count! |
|
add_block("FIFO_COUNTH", InvensenseV3DevReg::FIFO_COUNTH, 2, I2CRegisters::RegMode::RDONLY); |
|
|
|
add_fifo("FIFO_DATA", InvensenseV3DevReg::FIFO_DATA, I2CRegisters::RegMode::RDONLY); |
|
} |
|
|
|
void update(const class Aircraft &aircraft) override; |
|
|
|
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; |
|
|
|
virtual float accel_scale() const = 0; |
|
float gyro_scale() const { return (0.0174532f / 16.4f); } |
|
|
|
private: |
|
|
|
// sets the FIFO_COUNTH block based on the number of samples in FIFO_DATA |
|
void update_sample_count(); |
|
|
|
void assert_register_values(); |
|
|
|
void add_fifo(const char *name, uint8_t reg, I2CRegisters::RegMode mode); |
|
bool write_to_fifo(uint8_t fifo, uint8_t *value, uint8_t valuelen); |
|
|
|
int rdwr_fifo(I2C::i2c_rdwr_ioctl_data *&data); |
|
static const uint8_t fifo_len = 64; |
|
const char *fifoname[256]; |
|
Bitmask<256> writable_fifos; |
|
Bitmask<256> readable_fifos; |
|
|
|
// 256 pointers-to-malloced-values: |
|
char *values[256]; |
|
uint8_t value_lengths[256]; |
|
|
|
// swiped from the driver: |
|
struct PACKED FIFOData { |
|
uint8_t header; |
|
int16_t accel[3]; |
|
int16_t gyro[3]; |
|
int8_t temperature; |
|
uint16_t timestamp; |
|
}; |
|
|
|
// block support |
|
// for things that are larger than a register.... |
|
int rdwr_block(I2C::i2c_rdwr_ioctl_data *&data); |
|
void add_block(const char *name, uint8_t addr, uint8_t len, I2CRegisters::RegMode mode); |
|
void set_block(uint8_t addr, uint8_t *value, uint8_t valuelen); |
|
|
|
const char *blockname[256]; |
|
Bitmask<256> writable_blocks; |
|
Bitmask<256> readable_blocks; |
|
|
|
// 256 pointers-to-malloced-values: |
|
char *block_values[256]; |
|
uint8_t block_value_lengths[256]; |
|
}; |
|
|
|
} // namespace SITL
|
|
|