Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable the new sensor board for Saluki-FMU2 #736

Merged
merged 4 commits into from
Aug 14, 2024
+47 −64
Merged

Enable the new sensor board for Saluki-FMU2 #736

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ef8cf3f
Update ssrc submodules for a new sensor board
jlaitine Aug 9, 2024
2256cf4
icm40609d: Clear interrupt status at FIFO reset
jlaitine Aug 13, 2024
e3da03e
icm40609d: Add INTF register definition and disable I2C interface
jlaitine Aug 13, 2024
356c9e4
icm40609d: Change FIFO count to samples instead of bytes
jlaitine Aug 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion boards/ssrc/common
Open in desktop
Submodule common updated from d251df to 9d484d
2 changes: 1 addition & 1 deletion boards/ssrc/saluki-pi
Open in desktop
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Replacing icm42688p & icm20649 with icm40609d may cause problems. We have (a lot of) devices with old sensor board. How to handle those? We need detect these somehow.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Jaakko Isohella said that the new sensor board is the only one supported by FMU2. So we just drop the support for the old "rigid flex" board.

Submodule saluki-pi updated from 5efa56 to 453404
92 changes: 31 additions & 61 deletions src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -203,57 +203,45 @@ void ICM40609D::RunImpl()

case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
uint8_t samples = FIFOReadCount();

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;
if (samples == 0) {
perf_count(_fifo_empty_perf);

} else {
perf_count(_drdy_missed_perf);
} 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--;
}

// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}

if (samples == 0) {
// check current FIFO count
const uint16_t fifo_count = FIFOReadCount();

if (fifo_count >= FIFO::SIZE) {
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;
}
}

} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
bool success = false;

} else {
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
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);

// 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 ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_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;
} else {
perf_count(_drdy_missed_perf);
}
}
}

bool success = false;
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}

if (samples >= 1) {
if (FIFORead(timestamp_sample, samples)) {
success = true;

Expand Down Expand Up @@ -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;
}
}
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -599,6 +566,9 @@ void ICM40609D::FIFOReset()
// SIGNAL_PATH_RESET: FIFO flush
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);

// Read INT_STATUS to clear
RegisterRead(Register::BANK_0::INT_STATUS);

// reset while FIFO is disabled
_drdy_timestamp_sample.store(0);
}
Expand Down
3 changes: 2 additions & 1 deletion src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,10 +160,11 @@ class ICM40609D : public device::SPI, public I2CSPIDriver<ICM40609D>
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{10};
static constexpr uint8_t size_register_bank0_cfg{11};
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::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 },
Expand Down
12 changes: 12 additions & 0 deletions src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ enum class BANK_0 : uint8_t {

SIGNAL_PATH_RESET = 0x4B,

INTF_CONFIG0 = 0x4C,

PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
Expand Down Expand Up @@ -132,6 +134,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t {
FIFO_FLUSH = Bit1,
};

// INTF_CONFIG0
enum INTF_CONFIG0_BIT : uint8_t {
FIFO_HOLD_LAST_DATA_EN = Bit7,
FIFO_COUNT_REC = Bit6,
FIFO_COUNT_ENDIAN = Bit5,
SENSOR_DATA_ENDIAN = Bit4,
UI_SIFS_CFG_DISABLE_SPI = Bit1,
UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0
};

// PWR_MGMT0
enum PWR_MGMT0_BIT : uint8_t {
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
Expand Down