From d78696f0e2ecf791be5ae6da37bf4126d379d622 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?V=C3=A1clav=20Jel=C3=ADnek?= <jelinva4@fel.cvut.cz>
Date: Fri, 7 Mar 2025 11:01:59 +0100
Subject: [PATCH] Add reading IMU data from FIFO to prevent data loss due to
i2c bus being used by display
---
menu_programs/cube_gyro_acc.py | 5 +-
micropython/modules/lib/ICM42688.c | 122 +++++++++++++++++++----------
micropython/modules/lib/ICM42688.h | 13 +--
3 files changed, 89 insertions(+), 51 deletions(-)
diff --git a/menu_programs/cube_gyro_acc.py b/menu_programs/cube_gyro_acc.py
index cb98614..6492fab 100644
--- a/menu_programs/cube_gyro_acc.py
+++ b/menu_programs/cube_gyro_acc.py
@@ -57,10 +57,7 @@ def cube_gyro_acc_run(robot):
robot.display.text('< exit mode', 0, 54, 1)
robot.display.draw_arrow(74, 56, robot.display.UP, 1)
robot.display.draw_arrow(84, 61, robot.display.DOWN, 1)
- if counter % 2 == 0:
- time = ticks_ms()
- robot.display.show()
- print("tim:", (ticks_ms() - time))
+ robot.display.show()
# Exit program if left cube button is pressed_since
pressed = robot.buttons.pressed_since()
diff --git a/micropython/modules/lib/ICM42688.c b/micropython/modules/lib/ICM42688.c
index 1d6ab87..d65d16e 100644
--- a/micropython/modules/lib/ICM42688.c
+++ b/micropython/modules/lib/ICM42688.c
@@ -18,7 +18,8 @@ static ICM42688_t sensor;
static void ICM42688_read_data_irq(void);
// Read data from sensor and update AHRS algorithm
static void ICM42688_drdy_callback(void);
-static void ICM42688_calibrate_cycle(void);
+static void ICM42688_calibrate_cycle(int16_t gyro_x_raw, int16_t gyro_y_raw, int16_t gyro_z_raw);
+static void ICM42688_read_fifo(void);
static void ICM42688_fusion_cycle(void);
void ICM42688_init()
@@ -41,12 +42,12 @@ void ICM42688_init()
ICM42688_write_register(ICM42688_PWR_MGMT0, 0x0F); // Set to 0x0F to activate the sensor
sleep_ms(10);
// Configure the accelerometer
- ICM42688_write_register(ICM42688_ACCEL_CONFIG0, 0x4F); // Set to 0x2F for +/- 4g range, 500 Hz ODR
- ICM42688_write_register(ICM42688_ACCEL_CONFIG1, 0x10); // Set UI filter to 3rd order
+ ICM42688_write_register(ICM42688_ACCEL_CONFIG0, 0x47); // Set to 0x4F for +/- 4g range, 200 Hz ODR
+ ICM42688_write_register(ICM42688_ACCEL_CONFIG1, 0x08); // Set UI filter to 2nd order
// Configure the gyroscope
- ICM42688_write_register(ICM42688_GYRO_CONFIG0, 0x2F); // Set to 0x2F for +/- 500 deg/s range, 500 Hz ODR
- ICM42688_write_register(ICM42688_GYRO_CONFIG1, 0x08); // Set UI filter to 3rd order
+ ICM42688_write_register(ICM42688_GYRO_CONFIG0, 0x47); // Set to 0x4F for +/- 500 deg/s range, 200 Hz ODR
+ ICM42688_write_register(ICM42688_GYRO_CONFIG1, 0x08); // Set UI filter to 2nd order
ICM42688_write_register(ICM42688_GYROACCEL_CONFIG0, 0x00); // BW=ODR/2
@@ -55,21 +56,23 @@ void ICM42688_init()
ICM42688_write_register(ICM42688_INT_CONFIG0, 0x00); // Clear drdy int on Status Bit Read
ICM42688_write_register(ICM42688_INT_CONFIG1, 0x00); // Set int async reset to 0
ICM42688_write_register(ICM42688_INT_SOURCE0, 0x08); // Set to 0x10 to enable drdy interrupt
- ICM42688_write_register(ICM42688_INT_SOURCE3, 0x08); // Set to 0x10 to enable drdy interrupt
// Configure FIFO
ICM42688_write_register(ICM42688_FIFO_CONFIG, 0x40); // Set to 0x40 to stream-to-FIFO
- ICM42688_write_register(ICM42688_FIFO_CONFIG1, 0x43); // Enable partial reading and gyro and accel data in FIFO
+ ICM42688_write_register(ICM42688_FIFO_CONFIG1, 0x03); // Enable partial reading and gyro and accel data in FIFO
opencube_unlock_i2c();
for (int i = 0; i < 3; i++)
{
sensor.accel_offset[i] = 0.0;
sensor.gyro_offset[i] = 0.0;
+ sensor.accel[i] = 0.0;
+ sensor.gyro[i] = 0.0;
+ sensor.gyro_offset[i] = 0;
}
// Set the scale factors for the accelerometer and gyroscope
sensor.accel_scale = 4.0 / 32768.0; // +/- 4g range
- sensor.gyro_scale = 500.0 / 32768.0; // +/- 1000 deg/s range
+ sensor.gyro_scale = 500.0 / 32768.0; // +/- 500 deg/s range
sensor.calibrated = false;
sensor.calibrating = false;
@@ -134,6 +137,8 @@ void ICM42688_off()
opencube_lock_i2c_or_raise();
ICM42688_disable_interrupt(sensor);
ICM42688_bank_select(0);
+ ICM42688_write_register(ICM42688_DEVICE_CONFIG, 0x01); // Set reset bit
+ sleep_ms(2);
ICM42688_write_register(ICM42688_PWR_MGMT0, 0x00); // Disable gyro and accelerometer
opencube_unlock_i2c();
}
@@ -216,14 +221,12 @@ void ICM42688_read_data_irq(void)
opencube_unlock_i2c();
ICM42688_convert_accel_raw_to_float(accel_x_raw, accel_y_raw, accel_z_raw, &sensor.accel[0], &sensor.accel[1], &sensor.accel[2]);
ICM42688_convert_gyro_raw_to_float(gyro_x_raw, gyro_y_raw, gyro_z_raw, &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2]);
- } else {
-
}
}
void ICM42688_start_fusion(void) {
sensor.ahrs_enabled = true;
- FusionOffsetInitialise(&sensor.offset, 500);
+ FusionOffsetInitialise(&sensor.offset, ICM42688_ODR);
FusionAhrsInitialise(&sensor.ahrs);
}
@@ -267,28 +270,22 @@ bool ICM42688_calibrate_gyro(void) {
}
}
-void ICM42688_calibrate_cycle(void) {
- if (opencube_try_lock_i2c()) {
- int16_t gyro_x_raw, gyro_y_raw, gyro_z_raw;
- ICM42688_read_data_raw(NULL, NULL, NULL, &gyro_x_raw, &gyro_y_raw, &gyro_z_raw);
- opencube_unlock_i2c();
- sensor.gyro_cal_sum[0] += gyro_x_raw;
- sensor.gyro_cal_sum[1] += gyro_y_raw;
- sensor.gyro_cal_sum[2] += gyro_z_raw;
- sensor.calibration_cycles++;
- if (sensor.calibration_cycles >= CALIBRATION_CYCLES) {
- FusionOffsetInitialise(&sensor.offset, 500);
- sensor.gyro_offset[0] = (float)sensor.gyro_cal_sum[0] / CALIBRATION_CYCLES * sensor.gyro_scale;
- sensor.gyro_offset[1] = (float)sensor.gyro_cal_sum[1] / CALIBRATION_CYCLES * sensor.gyro_scale;
- sensor.gyro_offset[2] = (float)sensor.gyro_cal_sum[2] / CALIBRATION_CYCLES * sensor.gyro_scale;
- sensor.calibrating = false;
- sensor.calibrated = true;
- sensor.gyro_cal_sum[0] = 0;
- sensor.gyro_cal_sum[1] = 0;
- sensor.gyro_cal_sum[2] = 0;
- sensor.calibration_cycles = 0;
- }
-
+void ICM42688_calibrate_cycle(int16_t gyro_x_raw, int16_t gyro_y_raw, int16_t gyro_z_raw) {
+ sensor.gyro_cal_sum[0] += gyro_x_raw;
+ sensor.gyro_cal_sum[1] += gyro_y_raw;
+ sensor.gyro_cal_sum[2] += gyro_z_raw;
+ sensor.calibration_cycles++;
+ if (sensor.calibration_cycles >= CALIBRATION_CYCLES) {
+ FusionOffsetInitialise(&sensor.offset, ICM42688_ODR);
+ sensor.gyro_offset[0] = (float)sensor.gyro_cal_sum[0] / CALIBRATION_CYCLES * sensor.gyro_scale;
+ sensor.gyro_offset[1] = (float)sensor.gyro_cal_sum[1] / CALIBRATION_CYCLES * sensor.gyro_scale;
+ sensor.gyro_offset[2] = (float)sensor.gyro_cal_sum[2] / CALIBRATION_CYCLES * sensor.gyro_scale;
+ sensor.calibrating = false;
+ sensor.calibrated = true;
+ sensor.gyro_cal_sum[0] = 0;
+ sensor.gyro_cal_sum[1] = 0;
+ sensor.gyro_cal_sum[2] = 0;
+ sensor.calibration_cycles = 0;
}
}
@@ -297,7 +294,7 @@ void ICM42688_fusion_cycle(void) {
FusionVector gyroscope = { .axis = {.x = sensor.gyro[0], .y = sensor.gyro[1], .z = sensor.gyro[2]} };
gyroscope = FusionOffsetUpdate(&sensor.offset, gyroscope);
- FusionAhrsUpdateNoMagnetometer(&sensor.ahrs, gyroscope, accelerometer, 1.0 / 500.0);
+ FusionAhrsUpdateNoMagnetometer(&sensor.ahrs, gyroscope, accelerometer, 1.0 / ((float)ICM42688_ODR + ICM42688_ODR_DIFF));
FusionQuaternion q = FusionAhrsGetQuaternion(&sensor.ahrs);
q = FusionQuaternionMultiply(q, sensor.reset_quat);
FusionQuaternionNormalise(q);
@@ -306,16 +303,57 @@ void ICM42688_fusion_cycle(void) {
sensor.euler = FusionQuaternionToEuler(q);
}
-void ICM42688_drdy_callback(void) {
- uint32_t status = gpio_get_irq_event_mask(ICM42688_DRDY_PIN);
+void ICM42688_read_fifo(void) {
+ uint8_t count_buffer[2] = {0};
+ //uint8_t lost_count_buffer[2] = {0};
+ if (opencube_try_lock_i2c()) {
+ ICM42688_read_multiple_registers(ICM42688_FIFO_COUNT_H, count_buffer, 2);
+ //ICM42688_read_multiple_registers(ICM42688_FIFO_LOST_PKT_CNT, lost_count_buffer, 2);
+ opencube_unlock_i2c();
+ }
+ uint16_t fifo_count = ((uint16_t)(count_buffer[0] & 0x0F) << 8) | ((uint16_t)count_buffer[1]); // Convert to 16-bit
+ //uint16_t lost_count = ((uint16_t)(lost_count_buffer[0] & 0x0F) << 8) | ((uint16_t)lost_count_buffer[1]);
+ if (fifo_count > 16) {
+ //sensor.accel[1] += (float)fifo_count/ICM42688_PACKET_SIZE;
+ //sensor.accel[2] += (float)lost_count;
+ }
- ICM42688_read_data_irq();
- if (sensor.calibrating) {
- ICM42688_calibrate_cycle();
+ if (fifo_count > 0) {
+ uint8_t fifo_data[fifo_count];
+ if (opencube_try_lock_i2c()) {
+ ICM42688_read_multiple_registers(ICM42688_FIFO_DATA, fifo_data, fifo_count);
+ opencube_unlock_i2c();
+
+
+ int num_samples = fifo_count / ICM42688_PACKET_SIZE; // Each sample is 16 bytes (1 header + 6 accel + 6 gyro + 1 temp + 2 timestamp)
+ for (int i = 0; i < num_samples; i++) {
+ int16_t accel_x = (fifo_data[i * ICM42688_PACKET_SIZE + 1] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 2];
+ int16_t accel_y = (fifo_data[i * ICM42688_PACKET_SIZE + 3] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 4];
+ int16_t accel_z = (fifo_data[i * ICM42688_PACKET_SIZE + 5] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 6];
+ int16_t gyro_x = (fifo_data[i * ICM42688_PACKET_SIZE + 7] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 8];
+ int16_t gyro_y = (fifo_data[i * ICM42688_PACKET_SIZE + 9] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 10];
+ int16_t gyro_z = (fifo_data[i * ICM42688_PACKET_SIZE + 11] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 12];
+ //int16_t timestamp = (fifo_data[i * ICM42688_PACKET_SIZE + 14] << 8) | fifo_data[i * ICM42688_PACKET_SIZE + 15]; // Convert to 16-bit
+ ICM42688_convert_accel_raw_to_float(accel_x, accel_y, accel_z, &sensor.accel[0], &sensor.accel[1], &sensor.accel[2]);
+ ICM42688_convert_gyro_raw_to_float(gyro_x, gyro_y, gyro_z,&sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2]);
+
+ if (sensor.calibrating) {
+ ICM42688_calibrate_cycle(gyro_x, gyro_y, gyro_z);
+ }
+
+ if (sensor.ahrs_enabled) {
+ ICM42688_fusion_cycle();
+ //sensor.euler.angle.pitch = (float) fifo_data[13];
+ //sensor.euler.angle.roll = (float) timestamp;
+ }
+ }
+ }
}
+}
- if (sensor.ahrs_enabled) {
- ICM42688_fusion_cycle();
- }
+void ICM42688_drdy_callback(void) {
+ uint32_t status = gpio_get_irq_event_mask(ICM42688_DRDY_PIN);
+
+ ICM42688_read_fifo();
gpio_acknowledge_irq(ICM42688_DRDY_PIN, status);
}
\ No newline at end of file
diff --git a/micropython/modules/lib/ICM42688.h b/micropython/modules/lib/ICM42688.h
index 417da18..f5cbd20 100644
--- a/micropython/modules/lib/ICM42688.h
+++ b/micropython/modules/lib/ICM42688.h
@@ -71,14 +71,17 @@ Based on the ICM42688 datasheet: https://invensense.tdk.com/wp-content/uploads/2
#define ICM42688_FIFO_CONFIG1 0x5F
#define ICM42688_FIFO_CONFIG2 0x60
#define ICM42688_FIFO_CONFIG3 0x61
-#define ICM42688_FIFO_COUNT_H 0x70
-#define ICM42688_FIFO_COUNT_L 0x71
-#define ICM42688_FIFO_DATA 0x72
-
+#define ICM42688_FIFO_COUNT_H 0x2E
+#define ICM42688_FIFO_COUNT_L 0x2F
+#define ICM42688_FIFO_DATA 0x30
+#define ICM42688_FIFO_LOST_PKT_CNT 0x6C
+#define ICM42688_PACKET_SIZE 16
// Constants
#define ICM42688_WHO_AM_I_VALUE 0x47
-#define CALIBRATION_CYCLES 1000
+#define ICM42688_ODR 200
+#define ICM42688_ODR_DIFF (-2.3) // Observed difference between set and actual ODR, used for more accurate position approximation
+#define CALIBRATION_CYCLES (ICM42688_ODR*2)
typedef struct
{
i2c_inst_t *i2c_port; // I2C port of the sensor
--
GitLab