fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

icm40609d: Change FIFO count to samples instead of bytes

Browse Source 
As the sensor can directly report the amount of samples in the fifo, use it to simplify the logic.

Also combine the fifo empty/fifo overflow checks for interrupt and polling modes.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2024-08-12 16:08:10 +03:00 committed by Daniel Agar
parent cc4d5bd2a6
commit 0459481cb4
2 changed files with 32 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View File

@ -203,57 +203,45 @@ void ICM40609D::RunImpl()
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
perf_count(_drdy_missed_perf);
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
uint8_t samples = FIFOReadCount();
if (samples == 0) {
// check current FIFO count
const uint16_t fifo_count = FIFOReadCount();
perf_count(_fifo_empty_perf);
if (fifo_count >= FIFO::SIZE) {
} else {
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
} else {
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
samples--;
}
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;
}
samples = 0;
}
}
bool success = false;
if (samples >= 1) {
if (samples > 0) {
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
perf_count(_drdy_missed_perf);
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
if (FIFORead(timestamp_sample, samples)) {
success = true;
@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate)
void ICM40609D::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
for (auto &r : _register_bank0_cfg) {
if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
// FIFO_WM[7:0] FIFO_CONFIG2
r.set_bits = fifo_watermark_threshold & 0xFF;
r.set_bits = samples & 0xFF;
} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
// FIFO_WM[11:8] FIFO_CONFIG3
r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
}
}
}
@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
return false;
}
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
FIFOReset();
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
uint8_t valid_samples = 0;
for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
for (int i = 0; i < samples; i++) {
bool valid = true;
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx

2
src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
View File

@ -164,7 +164,7 @@ private:
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
{ Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0},
{ Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0},
{ Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },