zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

invensense icm20602 improvements 

- refactor Run() into simple state machine
 - perform reset and configuration in sensor bus thread
 - when using data ready interrupt skip checking FIFO count
sbg
Daniel Agar 5 years ago
parent
0c6d558c50
commit
afc59f843c
7 changed files with 409 additions and 263 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      boards/px4/fmu-v5/init/rc.board_sensors
  2. 1
      src/drivers/imu/invensense/icm20602/CMakeLists.txt
  3. 587
      src/drivers/imu/invensense/icm20602/ICM20602.cpp
  4. 73
      src/drivers/imu/invensense/icm20602/ICM20602.hpp
  5. 3
      src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
  6. 3
      src/lib/drivers/accelerometer/PX4Accelerometer.hpp
  7. 3
      src/lib/drivers/gyroscope/PX4Gyroscope.hpp

2
boards/px4/fmu-v5/init/rc.board_sensors
Unescape View File

@ -7,9 +7,11 @@ adc start
# Internal SPI bus ICM-20602 # Internal SPI bus ICM-20602
mpu6000 -R 8 -s -T 20602 start mpu6000 -R 8 -s -T 20602 start
#icm20602
# Internal SPI bus ICM-20689 # Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start mpu6000 -R 8 -z -T 20689 start
#icm20689 start
# Internal SPI bus BMI055 accel # Internal SPI bus BMI055 accel
bmi055 -A -R 10 start bmi055 -A -R 10 start

1
src/drivers/imu/invensense/icm20602/CMakeLists.txt
Unescape View File

@ -39,6 +39,7 @@ px4_add_module(
ICM20602.cpp ICM20602.cpp
ICM20602.hpp ICM20602.hpp
icm20602_main.cpp icm20602_main.cpp
InvenSense_ICM20602_registers.hpp
DEPENDS DEPENDS
drivers_accelerometer drivers_accelerometer
drivers_gyroscope drivers_gyroscope

587
src/drivers/imu/invensense/icm20602/ICM20602.cpp
Unescape View File

@ -42,11 +42,6 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
return (msb << 8u) | lsb; return (msb << 8u) | lsb;
} }
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
{
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
}
ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
@ -54,8 +49,11 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
{ {
set_device_type(DRV_ACC_DEVTYPE_ICM20602); set_device_type(DRV_ACC_DEVTYPE_ICM20602);
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602); _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602);
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
} }
ICM20602::~ICM20602() ICM20602::~ICM20602()
@ -75,36 +73,57 @@ ICM20602::~ICM20602()
perf_free(_drdy_interval_perf); perf_free(_drdy_interval_perf);
} }
void ICM20602::ConfigureSampleRate(int sample_rate) bool ICM20602::Init()
{ {
if (sample_rate == 0) { if (SPI::init() != PX4_OK) {
sample_rate = 1000; // default to 1 kHz PX4_ERR("SPI::init failed");
return false;
} }
sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz // allocate DMA capable buffer
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 500); // round down to nearest 250 us if (_dma_data_buffer == nullptr) {
_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); PX4_ERR("DMA alloc failed");
return false;
}
// recompute FIFO empty interval (us) with actual gyro sample limit return Reset();
_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); }
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); void ICM20602::Stop()
{
// wait until stopped
while (_state.load() != STATE::STOPPED) {
_state.store(STATE::REQUEST_STOP);
ScheduleNow();
px4_usleep(10);
}
}
_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); bool ICM20602::Reset()
_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); {
_state.store(STATE::RESET);
ScheduleClear();
ScheduleNow();
return true;
}
// FIFO watermark threshold in number of bytes void ICM20602::PrintInfo()
const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); {
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
for (auto &r : _register_cfg) { perf_print_counter(_transfer_perf);
if (r.reg == Register::FIFO_WM_TH1) { perf_print_counter(_bad_register_perf);
r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011; perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf);
} else if (r.reg == Register::FIFO_WM_TH2) { _px4_accel.print_status();
r.set_bits = fifo_watermark_threshold & 0xFF; _px4_gyro.print_status();
}
}
} }
int ICM20602::probe() int ICM20602::probe()
@ -119,47 +138,143 @@ int ICM20602::probe()
return PX4_OK; return PX4_OK;
} }
bool ICM20602::Init() void ICM20602::Run()
{ {
if (SPI::init() != PX4_OK) { switch (_state.load()) {
PX4_ERR("SPI::init failed"); case STATE::RESET:
return false; // PWR_MGMT_1: Device Reset
} RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_reset_timestamp = hrt_absolute_time();
// allocate DMA capable buffer _state.store(STATE::WAIT_FOR_RESET);
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE); ScheduleDelayed(100);
break;
if (_dma_data_buffer == nullptr) {
PX4_ERR("DMA alloc failed");
return false;
}
if (!Reset()) {
PX4_ERR("reset failed");
return false;
}
Start();
return true;
}
bool ICM20602::Reset() case STATE::WAIT_FOR_RESET:
{
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
for (int i = 0; i < 100; i++) {
// The reset value is 0x00 for all registers other than the registers below // The reset value is 0x00 for all registers other than the registers below
// Document Number: DS-000176 Page 31 of 57 // Document Number: DS-000176 Page 31 of 57
if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::PWR_MGMT_1) == 0x41) && (RegisterRead(Register::PWR_MGMT_1) == 0x41)
&& (RegisterRead(Register::CONFIG) == 0x80)) { && (RegisterRead(Register::CONFIG) == 0x80)) {
return true;
// if reset succeeded then configure
_state.store(STATE::CONFIGURE);
ScheduleNow();
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) {
PX4_ERR("Reset failed, retrying");
_state.store(STATE::RESET);
ScheduleDelayed(10_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 1 ms");
ScheduleDelayed(1_ms);
}
} }
}
return false; break;
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading from FIFO
_state.store(STATE::FIFO_READ);
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
FIFOReset();
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 1 ms
ScheduleDelayed(1_ms);
}
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// re-schedule as watchdog timeout
ScheduleDelayed(10_ms);
// timestamp set in data ready interrupt
samples = _fifo_read_samples.load();
timestamp_sample = _fifo_watermark_interrupt_timestamp;
}
bool failure = false;
// manually check FIFO count if no samples from DRDY or timestamp looks bogus
if (!_data_ready_interrupt_enabled || (samples == 0)
|| (hrt_elapsed_time(&timestamp_sample) > (_fifo_empty_interval_us / 2))) {
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
timestamp_sample = hrt_absolute_time();
const uint16_t fifo_count = FIFOReadCount();
if (fifo_count == 0) {
failure = true;
perf_count(_fifo_empty_perf);
}
samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
perf_count(_fifo_overflow_perf);
failure = true;
FIFOReset();
} else if (samples >= 2) {
// require at least 2 samples (we want at least 1 new accel sample per transfer)
if (!FIFORead(timestamp_sample, samples)) {
failure = true;
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check registers incrementally
if (RegisterCheck(_register_cfg[_checked_register], true)) {
_last_config_check_timestamp = timestamp_sample;
_checked_register = (_checked_register + 1) % size_register_cfg;
} else {
// register check failed, force reconfigure
PX4_DEBUG("Health check failed, reconfiguring");
_state.store(STATE::CONFIGURE);
ScheduleNow();
}
}
}
break;
case STATE::REQUEST_STOP:
DataReadyInterruptDisable();
ScheduleClear();
_state.store(STATE::STOPPED);
break;
case STATE::STOPPED:
// DO NOTHING
break;
}
} }
void ICM20602::ConfigureAccel() void ICM20602::ConfigureAccel()
@ -216,54 +331,126 @@ void ICM20602::ConfigureGyro()
} }
} }
void ICM20602::ResetFIFO() void ICM20602::ConfigureSampleRate(int sample_rate)
{ {
perf_count(_fifo_reset_perf); if (sample_rate == 0) {
sample_rate = 1000; // default to 1 kHz
}
// USER_CTRL: disable FIFO and reset all signal paths _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 250); // round down to nearest 250 us
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST, _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
USER_CTRL_BIT::FIFO_EN);
// FIFO_EN: enable both gyro and accel // recompute FIFO empty interval (us) with actual gyro sample limit
RegisterSetBits(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
// USER_CTRL: re-enable FIFO _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN,
USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST); _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
// FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA);
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_WM_TH1) {
r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011;
} else if (r.reg == Register::FIFO_WM_TH2) {
r.set_bits = fifo_watermark_threshold & 0xFF;
}
}
} }
bool ICM20602::Configure(bool notify) bool ICM20602::Configure()
{ {
bool success = true; bool success = true;
for (const auto &reg : _register_cfg) { for (const auto &reg : _register_cfg) {
if (!CheckRegister(reg, notify)) { if (!RegisterCheck(reg)) {
success = false; success = false;
} }
} }
ConfigureAccel();
ConfigureGyro();
return success; return success;
} }
bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify) int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<ICM20602 *>(arg)->DataReady();
return 0;
}
void ICM20602::DataReady()
{
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples);
ScheduleNow();
perf_count(_drdy_interval_perf);
}
bool ICM20602::DataReadyInterruptConfigure()
{
int ret_setevent = -1;
// Setup data ready on rising edge
// TODO: cleanup horrible DRDY define mess
#if defined(GPIO_DRDY_PORTC_PIN14)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_DRDY_ICM_2060X)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#endif
return (ret_setevent == 0);
}
bool ICM20602::DataReadyInterruptDisable()
{
int ret_setevent = -1;
// Disable data ready callback
// TODO: cleanup horrible DRDY define mess
#if defined(GPIO_DRDY_PORTC_PIN14)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_DRDY_ICM_2060X)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr);
#endif
return (ret_setevent == 0);
}
bool ICM20602::RegisterCheck(const register_config_t &reg_cfg, bool notify)
{ {
bool success = true; bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg); const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) { if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) {
if (notify) { PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
}
success = false; success = false;
} }
if (reg_cfg.clear_bits && (reg_value & reg_cfg.clear_bits)) { if (reg_cfg.clear_bits && (reg_value & reg_cfg.clear_bits)) {
if (notify) { PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
}
success = false; success = false;
} }
@ -279,6 +466,8 @@ bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify)
if (notify) { if (notify) {
perf_count(_bad_register_perf); perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
} }
} }
@ -325,170 +514,96 @@ void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits)
RegisterSetAndClearBits(reg, 0, clearbits); RegisterSetAndClearBits(reg, 0, clearbits);
} }
int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) uint16_t ICM20602::FIFOReadCount()
{
ICM20602 *dev = reinterpret_cast<ICM20602 *>(arg);
dev->DataReady();
return 0;
}
void ICM20602::DataReady()
{
perf_count(_drdy_interval_perf);
// make another measurement
ScheduleNow();
}
void ICM20602::Start()
{ {
ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); // read FIFO count
uint8_t fifo_count_buf[3] {};
// attempt to configure 3 times fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
for (int i = 0; i < 3; i++) {
if (Configure(false)) {
break;
}
}
// TODO: cleanup horrible DRDY define mess
#if defined(GPIO_DRDY_PORTC_PIN14)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_DRDY_ICM_2060X)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#else
_using_data_ready_interrupt_enabled = false;
ScheduleOnInterval(FIFO_EMPTY_INTERVAL_US, FIFO_EMPTY_INTERVAL_US);
#endif
ResetFIFO();
// schedule as watchdog if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
if (_using_data_ready_interrupt_enabled) { perf_count(_bad_transfer_perf);
ScheduleDelayed(100_ms); return 0;
} }
}
void ICM20602::Stop()
{
Reset();
// TODO: cleanup horrible DRDY define mess return combine(fifo_count_buf[1], fifo_count_buf[2]);
#if defined(GPIO_DRDY_PORTC_PIN14)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_DRDY_ICM_2060X)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr);
#endif
ScheduleClear();
} }
void ICM20602::Run() bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
{ {
// use the time now roughly corresponding with the last sample we'll pull from the FIFO TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
const hrt_abstime timestamp_sample = hrt_absolute_time(); const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
memset(report, 0, transfer_size);
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
// read FIFO count perf_begin(_transfer_perf);
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
return false;
} }
if (_using_data_ready_interrupt_enabled) { perf_end(_transfer_perf);
// re-schedule as watchdog
ScheduleDelayed(100_ms);
}
// check registers bool bad_data = false;
if (hrt_elapsed_time(&_last_config_check) > 100_ms) {
_checked_register = (_checked_register + 1) % size_register_cfg;
if (CheckRegister(_register_cfg[_checked_register])) { ProcessGyro(timestamp_sample, report, samples);
// delay next register check if current succeeded
_last_config_check = hrt_absolute_time();
} else { if (!ProcessAccel(timestamp_sample, report, samples)) {
// if register check failed reconfigure all bad_data = true;
Configure();
ResetFIFO();
return;
}
} }
const uint16_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); // limit temperature updates to 1 Hz
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
_temperature_update_timestamp = timestamp_sample;
if (samples < 2) { if (!ProcessTemperature(report, samples)) {
perf_count(_fifo_empty_perf); bad_data = true;
return; }
}
} else if (samples > FIFO_MAX_SAMPLES) { return !bad_data;
// not technically an overflow, but more samples than we expected or can publish }
perf_count(_fifo_overflow_perf);
ResetFIFO();
return; void ICM20602::FIFOReset()
} {
perf_count(_fifo_reset_perf);
// Transfer data // FIFO_EN: disable FIFO
struct TransferBuffer { RegisterWrite(Register::FIFO_EN, 0);
uint8_t cmd;
FIFO::DATA f[FIFO_MAX_SAMPLES];
};
// ensure no struct padding
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; // USER_CTRL: disable FIFO and reset all signal paths
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
memset(report, 0, transfer_size); USER_CTRL_BIT::FIFO_EN);
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
perf_begin(_transfer_perf); // reset while FIFO is disabled
_fifo_watermark_interrupt_timestamp = 0;
_fifo_read_samples.store(0);
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { // FIFO_EN: enable both gyro and accel
perf_end(_transfer_perf); // USER_CTRL: re-enable FIFO
perf_count(_bad_transfer_perf); for (const auto &r : _register_cfg) {
return; if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
}
} }
}
perf_end(_transfer_perf); static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
{
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
}
bool ICM20602::ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
{
PX4Accelerometer::FIFOSample accel; PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample; accel.timestamp_sample = timestamp_sample;
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
bool bad_data = false;
// accel data is doubled in FIFO, but might be shifted // accel data is doubled in FIFO, but might be shifted
int accel_first_sample = 0; int accel_first_sample = 1;
if (samples >= 3) { if (samples >= 3) {
if (fifo_accel_equal(report->f[0], report->f[1])) { if (fifo_accel_equal(report->f[0], report->f[1])) {
@ -503,7 +618,7 @@ void ICM20602::Run()
} else { } else {
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
return; bad_data = true;
} }
} }
@ -515,7 +630,8 @@ void ICM20602::Run()
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down) // sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel_samples] = accel_x; accel.x[accel_samples] = accel_x;
accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
@ -524,30 +640,44 @@ void ICM20602::Run()
accel.samples = accel_samples; accel.samples = accel_samples;
_px4_accel.updateFIFO(accel);
return !bad_data;
}
void ICM20602::ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
{
PX4Gyroscope::FIFOSample gyro; PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample; gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples; gyro.samples = samples;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
int16_t temperature[samples];
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = report->f[i]; const FIFO::DATA &fifo_sample = report->f[i];
temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L);
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down) // sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro_x; gyro.x[i] = gyro_x;
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
} }
// Temperature _px4_gyro.updateFIFO(gyro);
}
bool ICM20602::ProcessTemperature(const TransferBuffer *const report, uint8_t samples)
{
int16_t temperature[samples];
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = report->f[i];
temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L);
}
int32_t temperature_sum{0}; int32_t temperature_sum{0};
for (auto t : temperature) { for (auto t : temperature) {
@ -560,7 +690,7 @@ void ICM20602::Run()
// temperature changing wildly is an indication of a transfer error // temperature changing wildly is an indication of a transfer error
if (fabsf(t - temperature_avg) > 1000) { if (fabsf(t - temperature_avg) > 1000) {
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
return; return false;
} }
} }
@ -569,24 +699,5 @@ void ICM20602::Run()
_px4_accel.set_temperature(temperature_C); _px4_accel.set_temperature(temperature_C);
_px4_gyro.set_temperature(temperature_C); _px4_gyro.set_temperature(temperature_C);
return true;
_px4_gyro.updateFIFO(gyro);
_px4_accel.updateFIFO(accel);
}
void ICM20602::PrintInfo()
{
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }

73
src/drivers/imu/invensense/icm20602/ICM20602.hpp
Unescape View File

@ -48,6 +48,7 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp> #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h> #include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
using namespace InvenSense_ICM20602; using namespace InvenSense_ICM20602;
@ -66,6 +67,19 @@ public:
private: private:
// Sensor Configuration
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro
static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
// Transfer data
struct TransferBuffer {
uint8_t cmd;
FIFO::DATA f[FIFO_MAX_SAMPLES];
};
// ensure no struct padding
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
struct register_config_t { struct register_config_t {
Register reg; Register reg;
uint8_t set_bits{0}; uint8_t set_bits{0};
@ -74,17 +88,19 @@ private:
int probe() override; int probe() override;
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
void Run() override; void Run() override;
void ConfigureSampleRate(int sample_rate); bool Configure();
bool CheckRegister(const register_config_t &reg_cfg, bool notify = true);
bool Configure(bool notify = true);
void ConfigureAccel(); void ConfigureAccel();
void ConfigureGyro(); void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
uint8_t RegisterRead(Register reg); uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value); void RegisterWrite(Register reg, uint8_t value);
@ -92,7 +108,13 @@ private:
void RegisterSetBits(Register reg, uint8_t setbits); void RegisterSetBits(Register reg, uint8_t setbits);
void RegisterClearBits(Register reg, uint8_t clearbits); void RegisterClearBits(Register reg, uint8_t clearbits);
void ResetFIFO(); uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
bool ProcessTemperature(const TransferBuffer *const report, uint8_t samples);
uint8_t *_dma_data_buffer{nullptr}; uint8_t *_dma_data_buffer{nullptr};
@ -102,21 +124,30 @@ private:
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")}; perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")}; perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")}; perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
hrt_abstime _last_config_check{0}; hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _fifo_watermark_interrupt_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
px4::atomic<uint8_t> _fifo_read_samples{0};
bool _data_ready_interrupt_enabled{false};
uint8_t _checked_register{0}; uint8_t _checked_register{0};
bool _using_data_ready_interrupt_enabled{false}; enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
REQUEST_STOP,
STOPPED,
};
// Sensor Configuration px4::atomic<STATE> _state{STATE::RESET};
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro
static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
@ -135,6 +166,6 @@ private:
{ Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0] { Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0]
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 },
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 }, { Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 },
{ Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN, INT_ENABLE_BIT::DATA_RDY_INT_EN } { Register::INT_ENABLE, 0, INT_ENABLE_BIT::DATA_RDY_INT_EN }
}; };
}; };

3
src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
Unescape View File

@ -54,8 +54,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
namespace InvenSense_ICM20602 namespace InvenSense_ICM20602
{ {
static constexpr uint32_t SPI_SPEED = 10 * 1000 * static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
1000; // 10MHz SPI serial interface for communicating with all registers
static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x12; static constexpr uint8_t WHOAMI = 0x12;

3
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
Unescape View File

@ -58,7 +58,8 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype); void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; } void set_error_count(uint64_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_range(float range) { _range = range; UpdateClipLimit(); } void set_range(float range) { _range = range; UpdateClipLimit(); }
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
void set_temperature(float temperature) { _temperature = temperature; } void set_temperature(float temperature) { _temperature = temperature; }

3
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
Unescape View File

@ -60,7 +60,8 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype); void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; } void set_error_count(uint64_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_range(float range) { _range = range; UpdateClipLimit(); } void set_range(float range) { _range = range; UpdateClipLimit(); }
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
void set_temperature(float temperature) { _temperature = temperature; } void set_temperature(float temperature) { _temperature = temperature; }

Write Preview
Loading…
Cancel
Save