From b92b343d4e555f6290302373ea8e92ef8d2b3436 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 11 Jan 2021 11:10:01 +1100 Subject: [PATCH] SITL: add simulator for ICM40609 --- libraries/SITL/SIM_I2C.cpp | 3 + libraries/SITL/SIM_I2CDevice.cpp | 7 + libraries/SITL/SIM_I2CDevice.h | 3 + libraries/SITL/SIM_ICM40609.h | 23 +++ libraries/SITL/SIM_Invensense_v3.cpp | 216 +++++++++++++++++++++++++++ libraries/SITL/SIM_Invensense_v3.h | 94 ++++++++++++ 6 files changed, 346 insertions(+) create mode 100644 libraries/SITL/SIM_ICM40609.h create mode 100644 libraries/SITL/SIM_Invensense_v3.cpp create mode 100644 libraries/SITL/SIM_Invensense_v3.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index d16b518d53..6c6dcb85d7 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -27,6 +27,7 @@ #include "SIM_BattMonitor_SMBus_Rotoye.h" #include "SIM_Airspeed_DLVR.h" #include "SIM_Temperature_TSYS01.h" +#include "SIM_ICM40609.h" #include @@ -51,6 +52,7 @@ static Maxell maxell; static Rotoye rotoye; static Airspeed_DLVR airspeed_dlvr; static TSYS01 tsys01; +static ICM40609 icm40609; struct i2c_device_at_address { uint8_t bus; @@ -58,6 +60,7 @@ struct i2c_device_at_address { I2CDevice &device; } i2c_devices[] { { 0, 0x70, maxsonari2cxl }, + { 1, 0x01, icm40609 }, { 1, 0x55, toshibaled }, { 1, 0x38, ignored }, // NCP5623 { 1, 0x39, ignored }, // NCP5623C diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp index 3a9df8b7dc..584d8f00ad 100644 --- a/libraries/SITL/SIM_I2CDevice.cpp +++ b/libraries/SITL/SIM_I2CDevice.cpp @@ -145,6 +145,13 @@ int SITL::I2CRegisters_8Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data) }; +void SITL::I2CRegisters_8Bit::assert_register_value(uint8_t reg, uint8_t value) +{ + if (byte[reg] != value) { + AP_HAL::panic("Register 0x%02x (%s) was expected to have value (%02x) but has value (%02x)", reg, regname[reg], byte[reg], value); + } +} + int SITL::I2CCommandResponseDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data) { const uint32_t now = AP_HAL::millis(); diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h index 6fc6b48573..215786a088 100644 --- a/libraries/SITL/SIM_I2CDevice.h +++ b/libraries/SITL/SIM_I2CDevice.h @@ -49,6 +49,9 @@ public: return byte[(uint8_t)num]; } + // dies if register does not have value value + void assert_register_value(uint8_t reg, uint8_t value); + protected: uint8_t byte[256]; diff --git a/libraries/SITL/SIM_ICM40609.h b/libraries/SITL/SIM_ICM40609.h new file mode 100644 index 0000000000..121b82b957 --- /dev/null +++ b/libraries/SITL/SIM_ICM40609.h @@ -0,0 +1,23 @@ +#include "SIM_Invensense_v3.h" + +namespace SITL { + +class ICM40609DevReg : public InvensenseV3DevReg { +public: +}; + +class ICM40609 : public InvensenseV3 +{ +public: + void init() override { + InvensenseV3::init(); + + set_register(ICM40609DevReg::WHOAMI, (uint8_t)0x3b); + } + + float accel_scale() const override { return (GRAVITY_MSS / 1024); } + +private: +}; + +} // namespace SITL diff --git a/libraries/SITL/SIM_Invensense_v3.cpp b/libraries/SITL/SIM_Invensense_v3.cpp new file mode 100644 index 0000000000..ab1bb7f9ed --- /dev/null +++ b/libraries/SITL/SIM_Invensense_v3.cpp @@ -0,0 +1,216 @@ +#include "SIM_Invensense_v3.h" + +#include + +void SITL::InvensenseV3::update(const class Aircraft &aircraft) +{ + assert_storage_size _assert_fifo_size; + (void)_assert_fifo_size; + + const SITL *sitl = AP::sitl(); + const int16_t xAccel = sitl->state.xAccel / accel_scale(); + const int16_t yAccel = sitl->state.yAccel / accel_scale(); + const int16_t zAccel = sitl->state.zAccel / accel_scale(); + + const int16_t p = radians(sitl->state.rollRate) / gyro_scale(); + const int16_t q = radians(sitl->state.pitchRate) / gyro_scale(); + const int16_t r = radians(sitl->state.yawRate) / gyro_scale(); + + struct FIFOData new_data { + 0x68, + { xAccel, yAccel, zAccel }, + { p, q, r }, + 21, // temperature + AP_HAL::millis16() // timestamp + }; + + for (uint8_t i=0; i<2; i++) { + if (!write_to_fifo(InvensenseV3DevReg::FIFO_DATA, (uint8_t*)&new_data, sizeof(new_data))) { + return; + } + } + update_sample_count(); +} + +// assert_register_values ensures register states when we go to do +// various operations (e.g. reading from FIFO) +void SITL::InvensenseV3::assert_register_values() +{ + static const struct expected_register_values { + uint8_t reg; + uint8_t value; + } expected[] { + { InvensenseV3DevReg::FIFO_CONFIG, 0x80 }, + { InvensenseV3DevReg::FIFO_CONFIG1, 0x07 }, + { InvensenseV3DevReg::INTF_CONFIG0, 0xC0 }, + { InvensenseV3DevReg::SIGNAL_PATH_RESET, 2 }, + { InvensenseV3DevReg::PWR_MGMT0, 0x0f }, + { InvensenseV3DevReg::GYRO_CONFIG0, 0x05 }, + { InvensenseV3DevReg::ACCEL_CONFIG0, 0x05 }, + }; + for (const auto &stuff : expected) { + assert_register_value(stuff.reg, stuff.value); + } +} + +int SITL::InvensenseV3::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + const uint8_t addr = data->msgs[0].buf[0]; + + // see if it is a fifo... + if (fifoname[addr] != nullptr) { + return rdwr_fifo(data); + } + // see if it is a block... + if (blockname[addr] != nullptr) { + return rdwr_block(data); + } + + return I2CRegisters_8Bit::rdwr(data); +} + +int SITL::InvensenseV3::rdwr_fifo(I2C::i2c_rdwr_ioctl_data *&data) +{ + const uint8_t addr = data->msgs[0].buf[0]; + + assert_register_values(); + + // check for block/FIFO read/write bits and pieces + if (data->nmsgs == 2) { + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + if (data->msgs[1].flags != I2C_M_RD) { + AP_HAL::panic("Unexpected flags"); + } + + const uint8_t len = data->msgs[1].len; + if (len > value_lengths[addr]) { + if (value_lengths[addr] != 0) { + // we expect reads and writes into the fifo to be the same size + AP_HAL::panic("Read of unexpected size"); + } + return -1; + } + memcpy(data->msgs[1].buf, values[addr], len); + memmove(values[addr], values[addr]+len, value_lengths[addr]-len); + value_lengths[addr] -= len; + if (addr == InvensenseV3DevReg::FIFO_DATA) { // bit of a hack... callback? + update_sample_count(); + } + return 0; + } + return -1; +} + +void SITL::InvensenseV3::add_fifo(const char *name, uint8_t reg, int8_t mode) +{ + // ::fprintf(stderr, "Adding fifo %u (0x%02x) (%s)\n", reg, reg, name); + fifoname[reg] = name; + if (mode == O_RDONLY || mode == O_RDWR) { + readable_fifos.set((uint8_t)reg); + } + if (mode == O_WRONLY || mode == O_RDWR) { + writable_fifos.set((uint8_t)reg); + } + + values[reg] = (char*)malloc(fifo_len); // allocate the fifo... + if (values[reg] == nullptr) { + AP_HAL::panic("Failed to allocate FIFO..."); + } +} + +void SITL::InvensenseV3::update_sample_count() +{ + if (value_lengths[InvensenseV3DevReg::FIFO_DATA] % sizeof(FIFOData)) { + AP_HAL::panic("fifo data not multiple of sample size"); + } + uint16_t samplecount = value_lengths[InvensenseV3DevReg::FIFO_DATA]/sizeof(FIFOData); + set_block(InvensenseV3DevReg::FIFO_COUNTH, (uint8_t*)&samplecount, 2); +} + +bool SITL::InvensenseV3::write_to_fifo(uint8_t fifo, uint8_t *value, uint8_t valuelen) +{ + if (fifoname[fifo] == nullptr) { + AP_HAL::panic("Setting un-named fifo %u", fifo); + } + // ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value); + if (valuelen == 0) { + AP_HAL::panic("Zero-length values not permitted by spec"); + } + if (values[fifo] == nullptr) { + AP_HAL::panic("Write to unallocated FIFO"); + } + if (value_lengths[fifo] + valuelen > fifo_len) { + // ::fprintf(stderr, "dropped\n"); // this happens a lot at startup + return false; // just drop it + } + memcpy(&(values[fifo][value_lengths[fifo]]), value, valuelen); + value_lengths[fifo] += valuelen; + if (fifo == InvensenseV3DevReg::FIFO_DATA) { // bit of a hack... callback? + update_sample_count(); + } + return true; +} + + + +void SITL::InvensenseV3::add_block(const char *name, uint8_t addr, uint8_t len, int8_t mode) +{ + // ::fprintf(stderr, "Adding block %u (0x%02x) (%s)\n", addr, addr, name); + blockname[addr] = name; + block_values[addr] = (char*)malloc(len); + block_value_lengths[addr] = len; + if (block_values[addr] == nullptr) { + AP_HAL::panic("Allocation failed for block (len=%u)", len); + } + if (mode == O_RDONLY || mode == O_RDWR) { + readable_blocks.set((uint8_t)addr); + } + if (mode == O_WRONLY || mode == O_RDWR) { + writable_blocks.set((uint8_t)addr); + } +} + +void SITL::InvensenseV3::set_block(uint8_t addr, uint8_t *value, uint8_t valuelen) +{ + if (blockname[addr] == nullptr) { + AP_HAL::panic("Setting un-named block %u", addr); + } + if (valuelen != block_value_lengths[addr]) { + AP_HAL::panic("Invalid block write got=%u want=%u", valuelen, block_value_lengths[addr]); + } + memcpy(block_values[addr], value, valuelen); +} + +int SITL::InvensenseV3::rdwr_block(I2C::i2c_rdwr_ioctl_data *&data) +{ + const uint8_t addr = data->msgs[0].buf[0]; + + // it is a block. + if (data->nmsgs == 2) { + // data read request + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + if (data->msgs[1].flags != I2C_M_RD) { + AP_HAL::panic("Unexpected flags"); + } + + if (data->msgs[1].len != block_value_lengths[addr]) { + AP_HAL::panic("Block read length not equal to block length (got=%u want=%u)", data->msgs[1].len, block_value_lengths[addr]); + } + memcpy(&data->msgs[1].buf[0], block_values[addr], data->msgs[1].len); + return 0; + } + + if (data->nmsgs == 1) { + // data write request + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + AP_HAL::panic("block writes not implemented"); + } + + return -1; +} diff --git a/libraries/SITL/SIM_Invensense_v3.h b/libraries/SITL/SIM_Invensense_v3.h new file mode 100644 index 0000000000..141bdb9278 --- /dev/null +++ b/libraries/SITL/SIM_Invensense_v3.h @@ -0,0 +1,94 @@ +#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, O_RDONLY); + add_register("FIFO_CONFIG", InvensenseV3DevReg::FIFO_CONFIG, O_RDWR); + add_register("FIFO_CONFIG1", InvensenseV3DevReg::FIFO_CONFIG1, O_RDWR); + add_register("INTF_CONFIG0", InvensenseV3DevReg::INTF_CONFIG0, O_RDWR); + add_register("SIGNAL_PATH_RESET", InvensenseV3DevReg::SIGNAL_PATH_RESET, O_RDWR); + add_register("PWR_MGMT0", InvensenseV3DevReg::PWR_MGMT0, O_RDWR); + add_register("GYRO_CONFIG0", InvensenseV3DevReg::GYRO_CONFIG0, O_RDWR); + add_register("ACCDEL_CONFIG0", InvensenseV3DevReg::ACCEL_CONFIG0, O_RDWR); + + // sample count! + add_block("FIFO_COUNTH", InvensenseV3DevReg::FIFO_COUNTH, 2, O_RDONLY); + + add_fifo("FIFO_DATA", InvensenseV3DevReg::FIFO_DATA, O_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, int8_t 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, int8_t 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