mirror of https://github.com/ArduPilot/ardupilot
95 lines
3.4 KiB
C++
95 lines
3.4 KiB
C++
#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
|