Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add simulator for ICM40609

This commit is contained in:
Peter Barker 2021-01-11 11:10:01 +11:00 committed by Peter Barker
parent 239b09fb34
commit b92b343d4e
6 changed files with 346 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/SITL/SIM_I2C.cpp
View File

@ -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 <signal.h>
@ -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

7
libraries/SITL/SIM_I2CDevice.cpp
View File

@ -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();

3
libraries/SITL/SIM_I2CDevice.h
View File

@ -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];

23
libraries/SITL/SIM_ICM40609.h Normal file
View File

@ -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

216
libraries/SITL/SIM_Invensense_v3.cpp Normal file
View File

@ -0,0 +1,216 @@
#include "SIM_Invensense_v3.h"
#include <stdio.h>
void SITL::InvensenseV3::update(const class Aircraft &aircraft)
{
assert_storage_size<FIFOData, 16> _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;
}

94
libraries/SITL/SIM_Invensense_v3.h Normal file
View File

@ -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