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

icm42688p: accumulated minor improvements and cleanup 

- perform reset as per the datasheet (wakeup accel/gyro and wait before proceeding)
 - add register bank selection (not yet used)
 - track consecutive errors to trigger full reset if necessary
 - remove interrupt perf counter and instead only count misses
 - minor style changes to stay in sync with the other Invensense drivers
sbg
Daniel Agar 5 years ago
parent
a903eb7411
commit
8adea51131
3 changed files with 208 additions and 152 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 306
      src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
  2. 40
      src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
  3. 14
      src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp

306
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -48,6 +48,10 @@ ICM42688P::ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum @@ -48,6 +48,10 @@ ICM42688P::ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
_px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{
if (drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
}
@ -58,7 +62,7 @@ ICM42688P::~ICM42688P() @@ -58,7 +62,7 @@ ICM42688P::~ICM42688P()
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_interval_perf);
perf_free(_drdy_missed_perf);
}
int ICM42688P::init()
@ -76,6 +80,7 @@ int ICM42688P::init() @@ -76,6 +80,7 @@ int ICM42688P::init()
bool ICM42688P::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
ScheduleClear();
ScheduleNow();
return true;
@ -90,16 +95,15 @@ void ICM42688P::exit_and_cleanup() @@ -90,16 +95,15 @@ void ICM42688P::exit_and_cleanup()
void ICM42688P::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
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);
perf_print_counter(_drdy_missed_perf);
}
int ICM42688P::probe()
@ -116,26 +120,31 @@ int ICM42688P::probe() @@ -116,26 +120,31 @@ int ICM42688P::probe()
void ICM42688P::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// DEVICE_CONFIG: Software reset
// DEVICE_CONFIG: Software reset configuration
RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
_reset_timestamp = hrt_absolute_time();
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
break;
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
// if reset succeeded then configure
// Wakeup accel and gyro and schedule remaining configuration
RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
_state = STATE::CONFIGURE;
ScheduleNow();
ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
@ -157,7 +166,7 @@ void ICM42688P::RunImpl() @@ -157,7 +166,7 @@ void ICM42688P::RunImpl()
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
} else {
_data_ready_interrupt_enabled = false;
@ -167,75 +176,100 @@ void ICM42688P::RunImpl() @@ -167,75 +176,100 @@ void ICM42688P::RunImpl()
FIFOReset();
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 10 ms
ScheduleDelayed(10_ms);
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
uint8_t samples = 0;
uint32_t samples = 0;
if (_data_ready_interrupt_enabled) {
// re-schedule as watchdog timeout
ScheduleDelayed(10_ms);
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
// timestamp set in data ready interrupt
samples = _fifo_read_samples.load();
timestamp_sample = _fifo_watermark_interrupt_timestamp;
} else {
samples = _fifo_gyro_samples;
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
bool failure = false;
if (samples == 0) {
// check current FIFO count
const uint16_t fifo_count = FIFOReadCount();
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
// 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))) {
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
// 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();
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
} else {
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
samples = 0;
}
}
}
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 >= SAMPLES_PER_TRANSFER) {
// require at least SAMPLES_PER_TRANSFER (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();
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
if (_failure_count > 0) {
_failure_count--;
}
}
}
if (!success) {
_failure_count++;
} else if (samples == 0) {
failure = true;
perf_count(_fifo_empty_perf);
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
return;
}
}
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check BANK_0 registers incrementally
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)) {
_last_config_check_timestamp = timestamp_sample;
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
) {
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
} else {
// register check failed, force reconfigure
PX4_DEBUG("Health check failed, reconfiguring");
_state = STATE::CONFIGURE;
ScheduleNow();
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = timestamp_sample;
_temperature_update_timestamp = now;
}
}
}
@ -251,22 +285,22 @@ void ICM42688P::ConfigureAccel() @@ -251,22 +285,22 @@ void ICM42688P::ConfigureAccel()
switch (ACCEL_FS_SEL) {
case ACCEL_FS_SEL_2G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
_px4_accel.set_range(2 * CONSTANTS_ONE_G);
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_4G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
_px4_accel.set_range(4 * CONSTANTS_ONE_G);
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_8G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
_px4_accel.set_range(8 * CONSTANTS_ONE_G);
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_16G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
_px4_accel.set_range(16 * CONSTANTS_ONE_G);
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
break;
}
}
@ -309,17 +343,15 @@ void ICM42688P::ConfigureSampleRate(int sample_rate) @@ -309,17 +343,15 @@ void ICM42688P::ConfigureSampleRate(int sample_rate)
sample_rate = 800; // default to 800 Hz
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT;
// round down to nearest FIFO sample dt
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES);
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
ConfigureFIFOWatermark(_fifo_gyro_samples);
}
@ -340,12 +372,31 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples) @@ -340,12 +372,31 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
}
}
void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
{
if (bank != _last_register_bank) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
cmd_bank_sel[1] = bank;
transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
_last_register_bank = bank;
}
}
bool ICM42688P::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_bank0_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg : _register_bank0_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_bank0_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
@ -364,10 +415,11 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -364,10 +415,11 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM42688P::DataReady()
{
perf_count(_drdy_interval_perf);
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples);
ScheduleNow();
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
}
bool ICM42688P::DataReadyInterruptConfigure()
@ -377,7 +429,7 @@ bool ICM42688P::DataReadyInterruptConfigure() @@ -377,7 +429,7 @@ bool ICM42688P::DataReadyInterruptConfigure()
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &ICM42688P::DataReadyInterruptCallback, this) == 0;
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool ICM42688P::DataReadyInterruptDisable()
@ -389,7 +441,8 @@ bool ICM42688P::DataReadyInterruptDisable() @@ -389,7 +441,8 @@ bool ICM42688P::DataReadyInterruptDisable()
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool ICM42688P::RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify)
template <typename T>
bool ICM42688P::RegisterCheck(const T &reg_cfg)
{
bool success = true;
@ -405,47 +458,37 @@ bool ICM42688P::RegisterCheck(const register_bank0_config_t &reg_cfg, bool notif @@ -405,47 +458,37 @@ bool ICM42688P::RegisterCheck(const register_bank0_config_t &reg_cfg, bool notif
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
return success;
}
uint8_t ICM42688P::RegisterRead(Register::BANK_0 reg)
template <typename T>
uint8_t ICM42688P::RegisterRead(T reg)
{
uint8_t cmd[2] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
SelectRegisterBank(reg);
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
void ICM42688P::RegisterWrite(Register::BANK_0 reg, uint8_t value)
template <typename T>
void ICM42688P::RegisterWrite(T reg, uint8_t value)
{
uint8_t cmd[2] { (uint8_t)reg, value };
SelectRegisterBank(reg);
transfer(cmd, cmd, sizeof(cmd));
}
void ICM42688P::RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits)
template <typename T>
void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
}
uint8_t val = (orig_val & ~clearbits) | setbits;
if (clearbits) {
val &= ~clearbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
RegisterWrite(reg, val);
}
uint16_t ICM42688P::FIFOReadCount()
@ -453,6 +496,7 @@ uint16_t ICM42688P::FIFOReadCount() @@ -453,6 +496,7 @@ uint16_t ICM42688P::FIFOReadCount()
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
@ -462,29 +506,24 @@ uint16_t ICM42688P::FIFOReadCount() @@ -462,29 +506,24 @@ uint16_t ICM42688P::FIFOReadCount()
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_count(_bad_transfer_perf);
return false;
}
if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
@ -492,8 +531,15 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples) @@ -492,8 +531,15 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
return false;
}
const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
// check FIFO header in every sample
uint16_t valid_samples = 0;
uint8_t valid_samples = 0;
for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
bool valid = true;
@ -524,8 +570,8 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples) @@ -524,8 +570,8 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
}
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer, valid_samples);
ProcessAccel(timestamp_sample, buffer, valid_samples);
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true;
}
@ -540,52 +586,48 @@ void ICM42688P::FIFOReset() @@ -540,52 +586,48 @@ void ICM42688P::FIFOReset()
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
// reset while FIFO is disabled
_fifo_watermark_interrupt_timestamp = 0;
_fifo_read_samples.store(0);
_drdy_fifo_read_samples.store(0);
}
void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
int accel_samples = 0;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
int16_t accel_x = combine(fifo_sample.ACCEL_DATA_X1, fifo_sample.ACCEL_DATA_X0);
int16_t accel_y = combine(fifo_sample.ACCEL_DATA_Y1, fifo_sample.ACCEL_DATA_Y0);
int16_t accel_z = combine(fifo_sample.ACCEL_DATA_Z1, fifo_sample.ACCEL_DATA_Z0);
int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
// 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.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel_samples++;
accel.x[accel.samples] = accel_x;
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.samples++;
}
accel.samples = accel_samples;
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_accel.updateFIFO(accel);
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
}
}
void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
const int16_t gyro_x = combine(fifo_sample.GYRO_DATA_X1, fifo_sample.GYRO_DATA_X0);
const int16_t gyro_y = combine(fifo_sample.GYRO_DATA_Y1, fifo_sample.GYRO_DATA_Y0);
const int16_t gyro_z = combine(fifo_sample.GYRO_DATA_Z1, fifo_sample.GYRO_DATA_Z0);
const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
// 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)
@ -594,6 +636,9 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTrans @@ -594,6 +636,9 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTrans
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_gyro.updateFIFO(gyro);
}
@ -602,6 +647,7 @@ void ICM42688P::UpdateTemperature() @@ -602,6 +647,7 @@ void ICM42688P::UpdateTemperature()
// read current temperature
uint8_t temperature_buf[3] {};
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);

40
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
Unescape View File

@ -73,10 +73,9 @@ private: @@ -73,10 +73,9 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{125.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro
static constexpr float ACCEL_RATE{GYRO_RATE}; // 8 kHz accel
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
@ -108,25 +107,27 @@ private: @@ -108,25 +107,27 @@ private:
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
bool RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify = false);
uint8_t RegisterRead(Register::BANK_0 reg);
void RegisterWrite(Register::BANK_0 reg, uint8_t value);
void RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits);
void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
template <typename T> bool RegisterCheck(const T &reg_cfg);
template <typename T> uint8_t RegisterRead(T reg);
template <typename T> void RegisterWrite(T reg, uint8_t value);
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
void FIFOReset();
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio;
@ -139,14 +140,16 @@ private: @@ -139,14 +140,16 @@ private:
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_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_missed_perf{nullptr};
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};
int _failure_count{0};
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint8_t> _fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -159,8 +162,7 @@ private: @@ -159,8 +162,7 @@ private:
STATE _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{10};

14
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
Unescape View File

@ -42,6 +42,8 @@ @@ -42,6 +42,8 @@
#include <cstdint>
namespace InvenSense_ICM42688P
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
@ -52,8 +54,6 @@ static constexpr uint8_t Bit5 = (1 << 5); @@ -52,8 +54,6 @@ static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
namespace InvenSense_ICM42688P
{
static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
static constexpr uint8_t DIR_READ = 0x80;
@ -143,7 +143,7 @@ enum PWR_MGMT0_BIT : uint8_t { @@ -143,7 +143,7 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000 dps
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps
GYRO_FS_SEL_500_DPS = Bit6, // 0b010 = ±500 dps
GYRO_FS_SEL_250_DPS = Bit6 | Bit5, // 0b011 = ±250 dps
@ -207,6 +207,14 @@ enum INT_SOURCE0_BIT : uint8_t { @@ -207,6 +207,14 @@ enum INT_SOURCE0_BIT : uint8_t {
UI_AGC_RDY_INT1_EN = Bit0,
};
// REG_BANK_SEL
enum REG_BANK_SEL_BIT : uint8_t {
USER_BANK_0 = 0, // 0: Select USER BANK 0.
USER_BANK_1 = Bit4, // 1: Select USER BANK 1.
USER_BANK_2 = Bit5, // 2: Select USER BANK 2.
USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
};
namespace FIFO
{
static constexpr size_t SIZE = 2048;

Write Preview
Loading…
Cancel
Save