Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

invensense/icm42688p driver minor improvements

Browse Source 
- change default FIFO empty rate to 1 kHz (consistent with other drivers)
 - relax retry timeout if configure failed
 - improve configured empty rate (sample rate) rounding
 - check interupt status and FIFO count as part of full transfer and reset or adjust timing if necessary
This commit is contained in:
Daniel Agar
2020-03-25 00:04:05 -04:00
parent 669d2ec10b
commit 279d06ee50
2 changed files with 60 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+43 -41
View File
@@ -45,8 +45,8 @@ ICM42688P::ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency), SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_drdy_gpio(drdy_gpio), _drdy_gpio(drdy_gpio),
_px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation), _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{ {
set_device_type(DRV_IMU_DEVTYPE_ICM42688P); set_device_type(DRV_IMU_DEVTYPE_ICM42688P);
@@ -137,16 +137,17 @@ void ICM42688P::RunImpl()
case STATE::WAIT_FOR_RESET: case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
// if reset succeeded then configure // if reset succeeded then configure
_state = STATE::CONFIGURE; _state = STATE::CONFIGURE;
ScheduleNow(); ScheduleNow();
} else { } else {
// RESET not complete // RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) { if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_ERR("Reset failed, retrying"); PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET; _state = STATE::RESET;
ScheduleDelayed(10_ms); ScheduleDelayed(100_ms);
} else { } else {
PX4_DEBUG("Reset not complete, check again in 10 ms"); PX4_DEBUG("Reset not complete, check again in 10 ms");
@@ -204,13 +205,7 @@ void ICM42688P::RunImpl()
// use the time now roughly corresponding with the last sample we'll pull from the FIFO // use the time now roughly corresponding with the last sample we'll pull from the FIFO
timestamp_sample = hrt_absolute_time(); timestamp_sample = hrt_absolute_time();
const uint16_t fifo_count = FIFOReadCount(); const uint16_t fifo_count = FIFOReadCount();
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
if (fifo_count == 0) {
failure = true;
perf_count(_fifo_empty_perf);
}
samples = fifo_count / sizeof(FIFO::DATA);
} }
if (samples > FIFO_MAX_SAMPLES) { if (samples > FIFO_MAX_SAMPLES) {
@@ -219,18 +214,22 @@ void ICM42688P::RunImpl()
failure = true; failure = true;
FIFOReset(); FIFOReset();
} else if (samples >= 1) { } 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)) { if (!FIFORead(timestamp_sample, samples)) {
failure = true; failure = true;
_px4_accel.increase_error_count(); _px4_accel.increase_error_count();
_px4_gyro.increase_error_count(); _px4_gyro.increase_error_count();
} }
} else if (samples == 0) {
failure = true;
perf_count(_fifo_empty_perf);
} }
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check BANK_0 registers incrementally // check BANK_0 registers incrementally
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)) { if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)) {
_last_config_check_timestamp = timestamp_sample; _last_config_check_timestamp = timestamp_sample;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
@@ -308,7 +307,7 @@ void ICM42688P::ConfigureGyro()
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); _px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.0f)); _px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
} }
@@ -316,26 +315,30 @@ void ICM42688P::ConfigureGyro()
void ICM42688P::ConfigureSampleRate(int sample_rate) void ICM42688P::ConfigureSampleRate(int sample_rate)
{ {
if (sample_rate == 0) { if (sample_rate == 0) {
sample_rate = 2000; // default to 2 kHz sample_rate = 1000; // default to 1 kHz
} }
static constexpr float sample_min_interval = 1e6f / GYRO_RATE; // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / sample_min_interval) * sample_min_interval, _fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES);
sample_min_interval); // round down to nearest sample_min_interval
_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
// recompute FIFO empty interval (us) with actual gyro sample limit // recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
ConfigureFIFOWatermark(_fifo_gyro_samples);
}
void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO watermark threshold in number of bytes // FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
for (auto &r : _register_bank0_cfg) { for (auto &r : _register_bank0_cfg) {
if (r.reg == Register::BANK_0::FIFO_CONFIG2) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
@@ -373,10 +376,10 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM42688P::DataReady() void ICM42688P::DataReady()
{ {
perf_count(_drdy_interval_perf);
_fifo_watermark_interrupt_timestamp = hrt_absolute_time(); _fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples); _fifo_read_samples.store(_fifo_gyro_samples);
ScheduleNow(); ScheduleNow();
perf_count(_drdy_interval_perf);
} }
bool ICM42688P::DataReadyInterruptConfigure() bool ICM42688P::DataReadyInterruptConfigure()
@@ -400,9 +403,10 @@ bool ICM42688P::DataReadyInterruptDisable()
bool ICM42688P::RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify) bool ICM42688P::RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify)
{ {
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
bool success = true; bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false; success = false;
@@ -456,16 +460,6 @@ void ICM42688P::RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, u
RegisterWrite(reg, val); RegisterWrite(reg, val);
} }
void ICM42688P::RegisterSetBits(Register::BANK_0 reg, uint8_t setbits)
{
RegisterSetAndClearBits(reg, setbits, 0);
}
void ICM42688P::RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits)
{
RegisterSetAndClearBits(reg, 0, clearbits);
}
uint16_t ICM42688P::FIFOReadCount() uint16_t ICM42688P::FIFOReadCount()
{ {
// read FIFO count // read FIFO count
@@ -507,8 +501,14 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
return false; return false;
} }
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
// check FIFO header in every sample // check FIFO header in every sample
int valid_samples = 0; uint16_t valid_samples = 0;
for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
bool valid = true; bool valid = true;
@@ -559,7 +559,8 @@ void ICM42688P::FIFOReset()
_fifo_read_samples.store(0); _fifo_read_samples.store(0);
} }
void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
{ {
PX4Accelerometer::FIFOSample accel; PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample; accel.timestamp_sample = timestamp_sample;
@@ -586,7 +587,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTran
_px4_accel.updateFIFO(accel); _px4_accel.updateFIFO(accel);
} }
void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
{ {
PX4Gyroscope::FIFOSample gyro; PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample; gyro.timestamp_sample = timestamp_sample;
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+17 -13
View File
@@ -76,22 +76,24 @@ protected:
void custom_method(const BusCLIArguments &cli) override; void custom_method(const BusCLIArguments &cli) override;
void exit_and_cleanup() override; void exit_and_cleanup() override;
private: private:
// Sensor Configuration // Sensor Configuration
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro static constexpr float FIFO_SAMPLE_DT{125.f};
static constexpr uint32_t ACCEL_RATE{8000}; // 8 kHz accel static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; static constexpr float GYRO_RATE{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro
static constexpr float ACCEL_RATE{GYRO_RATE}; // 8 kHz accel
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
// Transfer data // Transfer data
struct FIFOTransferBuffer { struct FIFOTransferBuffer {
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ}; uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
uint8_t INT_STATUS; uint8_t INT_STATUS{0};
uint8_t FIFO_COUNTH; uint8_t FIFO_COUNTH{0};
uint8_t FIFO_COUNTL; uint8_t FIFO_COUNTL{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {}; FIFO::DATA f[FIFO_MAX_SAMPLES] {};
}; };
// ensure no struct padding // ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
struct register_bank0_config_t { struct register_bank0_config_t {
Register::BANK_0 reg; Register::BANK_0 reg;
@@ -105,6 +107,7 @@ private:
void ConfigureAccel(); void ConfigureAccel();
void ConfigureGyro(); void ConfigureGyro();
void ConfigureSampleRate(int sample_rate); void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
static int DataReadyInterruptCallback(int irq, void *context, void *arg); static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady(); void DataReady();
@@ -112,18 +115,19 @@ private:
bool DataReadyInterruptDisable(); bool DataReadyInterruptDisable();
bool RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify = false); bool RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify = false);
uint8_t RegisterRead(Register::BANK_0 reg); uint8_t RegisterRead(Register::BANK_0 reg);
void RegisterWrite(Register::BANK_0 reg, uint8_t value); void RegisterWrite(Register::BANK_0 reg, uint8_t value);
void RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits); void RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits);
void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits); void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits); void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount(); uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples); bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
void FIFOReset(); void FIFOReset();
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples); void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples); void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void UpdateTemperature(); void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio; const spi_drdy_gpio_t _drdy_gpio;
@@ -156,7 +160,7 @@ private:
STATE _state{STATE::RESET}; STATE _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{500}; // default 500 us / 2000 Hz transfer interval uint16_t _fifo_empty_interval_us{1000}; // default 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))};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};