fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 15:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

invensense/icm40609d 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:02:46 -04:00
parent c6825aa177
commit 669d2ec10b
2 changed files with 59 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+42 -41
View File
@@ -45,8 +45,8 @@ ICM40609D::ICM40609D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_drdy_gpio(drdy_gpio),
_px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
_px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{
set_device_type(DRV_IMU_DEVTYPE_ICM40609D);
@@ -142,10 +142,10 @@ void ICM40609D::RunImpl()
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) {
PX4_ERR("Reset failed, retrying");
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
@@ -203,13 +203,7 @@ void ICM40609D::RunImpl()
// 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);
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
}
if (samples > FIFO_MAX_SAMPLES) {
@@ -218,18 +212,22 @@ void ICM40609D::RunImpl()
failure = true;
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)) {
failure = true;
_px4_accel.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) {
// check BANK_0 registers incrementally
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)) {
_last_config_check_timestamp = timestamp_sample;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
@@ -307,7 +305,7 @@ void ICM40609D::ConfigureGyro()
case GYRO_FS_SEL_2000_DPS:
_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;
}
}
@@ -315,26 +313,30 @@ void ICM40609D::ConfigureGyro()
void ICM40609D::ConfigureSampleRate(int sample_rate)
{
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,
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);
_fifo_gyro_samples = 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 * (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_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
ConfigureFIFOWatermark(_fifo_gyro_samples);
}
void ICM40609D::ConfigureFIFOWatermark(uint8_t samples)
{
// 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) {
if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
@@ -372,10 +374,10 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM40609D::DataReady()
{
perf_count(_drdy_interval_perf);
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples);
ScheduleNow();
perf_count(_drdy_interval_perf);
}
bool ICM40609D::DataReadyInterruptConfigure()
@@ -399,9 +401,10 @@ bool ICM40609D::DataReadyInterruptDisable()
bool ICM40609D::RegisterCheck(const register_bank0_config_t &reg_cfg, bool notify)
{
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
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)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
@@ -455,16 +458,6 @@ void ICM40609D::RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, u
RegisterWrite(reg, val);
}
void ICM40609D::RegisterSetBits(Register::BANK_0 reg, uint8_t setbits)
{
RegisterSetAndClearBits(reg, setbits, 0);
}
void ICM40609D::RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits)
{
RegisterSetAndClearBits(reg, 0, clearbits);
}
uint16_t ICM40609D::FIFOReadCount()
{
// read FIFO count
@@ -506,8 +499,14 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
return false;
}
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
// 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++) {
bool valid = true;
@@ -558,7 +557,8 @@ void ICM40609D::FIFOReset()
_fifo_read_samples.store(0);
}
void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
{
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
@@ -585,7 +585,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTran
_px4_accel.updateFIFO(accel);
}
void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
{
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
+17 -13
View File
@@ -76,22 +76,24 @@ protected:
void custom_method(const BusCLIArguments &cli) override;
void exit_and_cleanup() override;
private:
// Sensor Configuration
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro
static constexpr uint32_t ACCEL_RATE{8000}; // 8 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]))};
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 uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
uint8_t INT_STATUS;
uint8_t FIFO_COUNTH;
uint8_t FIFO_COUNTL;
uint8_t INT_STATUS{0};
uint8_t FIFO_COUNTH{0};
uint8_t FIFO_COUNTL{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// 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 {
Register::BANK_0 reg;
@@ -105,6 +107,7 @@ private:
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
@@ -112,18 +115,19 @@ private:
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);
void RegisterClearBits(Register::BANK_0 reg, 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); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
void FIFOReset();
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
void ProcessGyro(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, const uint8_t samples);
void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio;
@@ -156,7 +160,7 @@ private:
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_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};