From f79c444f0774a2bfeef6eb2f23a749b782a9692a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 11 Aug 2026 05:36:31 +0300 Subject: [PATCH] Configure imu using parameters instead of editing the source code Choose the model and optionally custom pin numbers. --- docs/usage.md | 20 ++++++++------- flix/imu.ino | 59 ++++++++++++++++++++++++++++++--------------- flix/parameters.ino | 9 +++++++ gazebo/flix.h | 3 +++ 4 files changed, 62 insertions(+), 29 deletions(-) diff --git a/docs/usage.md b/docs/usage.md index 5e7a0a8..c6914c3 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,17 @@ 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 IMU: + * `IMU_MODEL` — IMU model (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 (should print `status: OK`). + ### 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 b6a84b5..0aafb13 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -4,12 +4,17 @@ // Work with the IMU sensor #include +#include #include #include "vector.h" #include "lpf.h" #include "util.h" -MPU9250 imu(SPI); +IMU *imu; +int imuModel = -1; // 1 - MPU9250, 2 - ICM20948, 3 - MPU6050, 4 - ICM40609D +int imuBus = 0; // 0 - SPI, 1 - I2C +int imuSckPin = SCK, imuMisoPin = MISO, imuMosiPin = MOSI, imuCsPin = SS, 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,22 +28,35 @@ LowPassFilter gyroBiasFilter(0.001); void setupIMU() { print("Setup IMU\n"); - imu.begin(); + free(imu); + if (imuModel == 3) imuBus = 1; // MPU6050 is I2C only + + if (imuBus == 0) { + // SPI connection + SPI.begin(imuSckPin, imuMisoPin, imuMosiPin); + imu = IMU::create(imuModel, SPI, imuCsPin, imuIntPin); + } else { + // I2C connection + Wire.setPins(imuSdaPin, imuSclPin); + imu = IMU::create(imuModel, Wire, imuIntPin); + } + + imu->begin(); configureIMU(); } void configureIMU() { - imu.setAccelRange(imu.ACCEL_RANGE_4G); - imu.setGyroRange(imu.GYRO_RANGE_2000DPS); - imu.setDLPF(imu.DLPF_MAX); - imu.setRate(imu.RATE_1KHZ_APPROX); - imu.setupInterrupt(); + imu->setAccelRange(IMU::ACCEL_RANGE_4G); + imu->setGyroRange(IMU::GYRO_RANGE_2000DPS); + imu->setDLPF(IMU::DLPF_MAX); + imu->setRate(IMU::RATE_1KHZ_APPROX); + imu->setupInterrupt(); } void readIMU() { - imu.waitForData(); - imu.getGyro(gyro.x, gyro.y, gyro.z); - imu.getAccel(acc.x, acc.y, acc.z); + imu->waitForData(); + imu->getGyro(gyro.x, gyro.y, gyro.z); + imu->getAccel(acc.x, acc.y, acc.z); calibrateGyroOnce(); // Apply scale and bias @@ -60,7 +78,7 @@ void calibrateGyroOnce() { void calibrateAccel() { print("Calibrating accelerometer\n"); - imu.setAccelRange(imu.ACCEL_RANGE_2G); // the most sensitive mode + imu->setAccelRange(IMU::ACCEL_RANGE_2G); // the most sensitive mode print("1/6 Place level [8 sec]\n"); pause(8); @@ -94,9 +112,9 @@ void calibrateAccelOnce() { // Compute the average of the accelerometer readings acc = Vector(0, 0, 0); for (int i = 0; i < samples; i++) { - imu.waitForData(); + imu->waitForData(); Vector sample; - imu.getAccel(sample.x, sample.y, sample.z); + imu->getAccel(sample.x, sample.y, sample.z); acc = acc + sample; } acc = acc / samples; @@ -121,17 +139,18 @@ void printIMUCalibration() { } void printIMUInfo() { - imu.status() ? print("status: ERROR %d\n", imu.status()) : print("status: OK\n"); - print("model: %s\n", imu.getModel()); - print("who am I: 0x%02X\n", imu.whoAmI()); + imu->status() ? print("status: ERROR %d\n", imu->status()) : print("status: OK\n"); + print("model: %s\n", imu->getModel()); + print("who am I: 0x%02X\n", imu->whoAmI()); print("rate: %.0f\n", loopRate); - print("temperature: %.1f °C\n", imu.getTemp()); + 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); - imu.waitForData(); + imu->waitForData(); Vector rawGyro, rawAcc; - imu.getGyro(rawGyro.x, rawGyro.y, rawGyro.z); - imu.getAccel(rawAcc.x, rawAcc.y, rawAcc.z); + imu->getGyro(rawGyro.x, rawGyro.y, rawGyro.z); + imu->getAccel(rawAcc.x, rawAcc.y, rawAcc.z); print("raw gyro: %f %f %f\n", rawGyro.x, rawGyro.y, rawGyro.z); print("raw acc: %f %f %f\n", rawAcc.x, rawAcc.y, rawAcc.z); } diff --git a/flix/parameters.ino b/flix/parameters.ino index acac658..daed501 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -61,6 +61,15 @@ 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_SDA", &imuSdaPin}, + {"IMU_PIN_SCL", &imuSclPin}, + {"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 1d88343..62593e7 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -21,6 +21,9 @@ extern float motors[4]; Vector gyro, acc, imuRotation; Vector accBias, gyroBias, accScale(1, 1, 1); LowPassFilter gyroBiasFilter(0); +int imuModel = 1, imuBus = 0; +int imuSckPin = 0, imuMisoPin = 0, imuMosiPin = 0, imuCsPin = -1, imuIntPin = -1; +int imuSdaPin = 0, imuSclPin = 0; // declarations void step();