ardupilot/libraries/SITL/SIM_Invensense_v3.cpp

217 lines
6.8 KiB
C++

#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;
}