From 0a53170097505b3457d2d78f514d2b01ead66f13 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sat, 8 Aug 2026 01:24:43 +0300
Subject: [PATCH] Make imu configuration parameters work

---
 flix/imu.ino | 48 ++++++++++++++++++++++++++----------------------
 flix/log.ino |  2 +-
 2 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/flix/imu.ino b/flix/imu.ino
index 23f3097..e4145d1 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -10,8 +10,9 @@
 #include "filter.h"
 #include "util.h"
 
-IMU imu;
-int imuModel = 1, imuBus = 0;
+IMU *imu;
+int imuModel = 1;
+int imuBus = 0; // 0 - SPI, 1 - I2C
 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
@@ -27,30 +28,33 @@ LowPassFilter<Vector> gyroBiasFilter(0.001);
 
 void setupIMU() {
 	print("Setup IMU\n");
+	free(imu);
 	if (imuBus == 0 && imuCsPin > 0) {
 		// SPI connection
 		SPI.begin(imuSckPin, imuMisoPin, imuMosiPin);
-		imu.begin((IMU::Model)imuModel, SPI, imuCsPin, imuIntPin);
+		imu = IMU::create(imuModel, SPI, imuCsPin, imuIntPin);
+		imu->begin();
 	} else if (imuBus == 1) {
 		// I2C connection
 		Wire.setPins(imuSdaPin, imuSclPin);
-		imu.begin((IMU::Model)imuModel, Wire, imuIntPin);
+		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
@@ -72,7 +76,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);
@@ -106,9 +110,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;
@@ -133,18 +137,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("interrupt mode: %s\n", imuIntPin != -1 ? "pin" : "timer");
-	print("temperature: %.1f °C\n", imu.getTemp());
+	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/log.ino b/flix/log.ino
index b4f94e4..1834414 100644
--- a/flix/log.ino
+++ b/flix/log.ino
@@ -100,7 +100,7 @@ LogTopic logTopics[] = {
 	LogTopic(5,
 		{"voltage", &voltage},
 		{"temp", &temperatureRead},
-		{"imuTemp", []() { return imu.getTemp(); }}),
+		{"imuTemp", []() { return imu->getTemp(); }}),
 };
 
 void *logBuffer; // buffer for log data