From 1203a3ae8f41e75c197fbefba23ad7039febf500 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Thu, 16 Jul 2026 23:09:07 +0300
Subject: [PATCH] Make imu model and pins configurable using parameters

---
 docs/usage.md       | 22 +++++++++++++---------
 flix/imu.ino        | 17 +++++++++++++++--
 flix/parameters.ino |  7 +++++++
 gazebo/flix.h       |  4 ++++
 4 files changed, 39 insertions(+), 11 deletions(-)

diff --git a/docs/usage.md b/docs/usage.md
index 2f05315..02e3a63 100644
--- a/docs/usage.md
+++ b/docs/usage.md
@@ -71,15 +71,6 @@ See other available Make commands in [Makefile](../Makefile).
 
 ## Before first flight
 
-### Choose the IMU model
-
-In case if using different IMU model than MPU9250, change `imu` variable declaration in the `imu.ino`:
-
-```cpp
-ICM20948 imu(SPI);  // For ICM-20948
-MPU6050 imu(Wire);  // For MPU-6050
-```
-
 ### Connect using QGroundControl
 
 QGroundControl is a ground control station software that can be used to monitor and control the drone.
@@ -120,6 +111,19 @@ The drone is configured using parameters. To access and modify them, go to the Q
 
 You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
 
+### Configure the IMU
+
+1. Configure the following parameters for the used IMU:
+   * `IMU_MODEL` — IMU model (0 for disabled, 1 for MPU-9250/MPU-6500¹, 2 for ICM-20948, 3 for MPU-6050, 4 for ICM-40609-D).
+   * `IMU_BUS` — communication bus (0 for SPI, 1 for I²C).
+   * `IMU_PIN_SCK`, `IMU_PIN_MISO`, `IMU_PIN_MOSI`, `IMU_PIN_CS` — SPI pin numbers.
+   * `IMU_PIN_SCL`, `IMU_PIN_SDA` — I²C pin numbers.
+   * `IMU_PIN_INT` — IMU data ready pin number (-1 if not used).
+2. Reboot the drone.
+3. Check the IMU is working using `imu` command in the console (see details below).
+
+¹ — not to be confused with MPU-6050.
+
 ### Define IMU orientation
 
 The IMU orientation (relative to the drone's axes) is defined using the parameters: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
diff --git a/flix/imu.ino b/flix/imu.ino
index 37aa1c2..23f3097 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -4,12 +4,16 @@
 // Work with the IMU sensor
 
 #include <SPI.h>
+#include <Wire.h>
 #include <FlixPeriph.h>
 #include "vector.h"
 #include "filter.h"
 #include "util.h"
 
-MPU9250 imu(SPI);
+IMU imu;
+int imuModel = 1, imuBus = 0;
+int imuSckPin = SCK, imuMisoPin = MISO, imuMosiPin = MOSI, imuCsPin = -1, imuIntPin = -1;
+int imuSdaPin = SDA, imuSclPin = SCL;
 Vector imuRotation(0, 0, PI / 2); // imu orientation as Euler angles
 
 Vector gyro; // gyroscope output, rad/s
@@ -23,7 +27,15 @@ LowPassFilter<Vector> gyroBiasFilter(0.001);
 
 void setupIMU() {
 	print("Setup IMU\n");
-	imu.begin();
+	if (imuBus == 0 && imuCsPin > 0) {
+		// SPI connection
+		SPI.begin(imuSckPin, imuMisoPin, imuMosiPin);
+		imu.begin((IMU::Model)imuModel, SPI, imuCsPin, imuIntPin);
+	} else if (imuBus == 1) {
+		// I2C connection
+		Wire.setPins(imuSdaPin, imuSclPin);
+		imu.begin((IMU::Model)imuModel, Wire, imuIntPin);
+	}
 	configureIMU();
 }
 
@@ -125,6 +137,7 @@ void printIMUInfo() {
 	print("model: %s\n", imu.getModel());
 	print("who am I: 0x%02X\n", imu.whoAmI());
 	print("rate: %.0f\n", loopRate);
+	print("interrupt mode: %s\n", imuIntPin != -1 ? "pin" : "timer");
 	print("temperature: %.1f °C\n", imu.getTemp());
 	print("gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z);
 	print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
diff --git a/flix/parameters.ino b/flix/parameters.ino
index 76e8ebe..0c7da90 100644
--- a/flix/parameters.ino
+++ b/flix/parameters.ino
@@ -60,6 +60,13 @@ Parameter parameters[] = {
 	{"CTL_FLT_MODE_1", &flightModes[1]},
 	{"CTL_FLT_MODE_2", &flightModes[2]},
 	// imu
+	{"IMU_MODEL", &imuModel},
+	{"IMU_BUS", &imuBus},
+	{"IMU_PIN_SCK", &imuSckPin},
+	{"IMU_PIN_MISO", &imuMisoPin},
+	{"IMU_PIN_MOSI", &imuMosiPin},
+	{"IMU_PIN_CS", &imuCsPin},
+	{"IMU_PIN_INT", &imuIntPin},
 	{"IMU_ROT_ROLL", &imuRotation.x},
 	{"IMU_ROT_PITCH", &imuRotation.y},
 	{"IMU_ROT_YAW", &imuRotation.z},
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 49f0f70..265fa9e 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -21,6 +21,10 @@ extern float motors[4];
 Vector gyro, acc, imuRotation;
 Vector accBias, gyroBias, accScale(1, 1, 1);
 LowPassFilter<Vector> gyroBiasFilter(0);
+int imuModel = 1, imuBus = 0;
+int imuSckPin = 0, imuMisoPin = 0, imuMosiPin = 0, imuCsPin = -1, imuIntPin = -1;
+int imuSdaPin = 0, imuSclPin = 0;
+int rgbPin = -1, rgbNum = 1;
 
 // declarations
 void step();