Use FlixPeriph library for IMU, implement own IMU calibration

Makefile

@@ -16,7 +16,7 @@ dependencies .dependencies:
 	arduino-cli core install esp32:esp32@2.0.11 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
 	arduino-cli lib install "Bolder Flight Systems SBUS"@1.0.1
-	arduino-cli lib install "Bolder Flight Systems MPU9250"@1.0.2
+	arduino-cli lib install "FlixPeriph"
 	arduino-cli lib install "MAVLink"@2.0.1
 	touch .dependencies
 

docs/build.md

@@ -83,7 +83,7 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set
 2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores).
 3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) (**versions are significant**):
    * `Bolder Flight Systems SBUS`, version 1.0.1.
-   * `Bolder Flight Systems MPU9250`, version 1.0.2.
+   * `FlixPeriph`, version 1.0.0.
    * `MAVLink`, version 2.0.1.
 4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
 5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.

flix/flix.ino

@@ -51,8 +51,7 @@ void setup() {
 }
 
 void loop() {
-	if (!readIMU()) return;
+	readIMU();
-
 	step();
 	readRC();
 	estimate();

flix/imu.ino

@@ -6,86 +6,116 @@
 #include <SPI.h>
 #include <MPU9250.h>
 
-#define LOAD_GYRO_CAL false
+#define ONE_G 9.80665
 
-MPU9250 IMU(SPI, SS);
+// NOTE: use 'ca' command to calibrate the accelerometer and put the values here
+Vector accBias(0, 0, 0);
+Vector accScale(1, 1, 1);
+
+MPU9250 IMU(SPI, 4);
+Vector gyroBias;
 
 void setupIMU() {
-	Serial.println("Setup IMU, stand still");
+	Serial.println("Setup IMU");
-
+	bool status = IMU.begin();
-	auto status = IMU.begin();
+	if (!status) {
-	if (status < 0) {
 		while (true) {
-			Serial.printf("IMU begin error: %d\n", status);
+			Serial.println("IMU begin error");
 			delay(1000);
 		}
 	}
+	calibrateGyro();
+}
 
-	if (LOAD_GYRO_CAL) loadGyroCal();
+void configureIMU() {
-	loadAccelCal();
+	IMU.setAccelRange(IMU.ACCEL_RANGE_4G);
-
+	IMU.setGyroRange(IMU.GYRO_RANGE_500DPS);
+	IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);
 	IMU.setSrd(0); // set sample rate to 1000 Hz
-	// NOTE: very important, without the above the rate would be terrible 50 Hz
 }
 
-bool readIMU() {
+void readIMU() {
-	if (IMU.readSensor() < 0) {
+	IMU.waitForData();
-		Serial.println("IMU read error");
+	IMU.getGyro(rates.x, rates.y, rates.z);
-		return false;
+	IMU.getAccel(acc.x, acc.y, acc.z);
-	}
+	// apply scale and bias
-
+	acc = (acc - accBias) / accScale;
-	auto lastRates = rates;
+	rates = rates - gyroBias;
-
-	rates.x = IMU.getGyroX_rads();
-	rates.y = IMU.getGyroY_rads();
-	rates.z = IMU.getGyroZ_rads();
-	acc.x = IMU.getAccelX_mss();
-	acc.y = IMU.getAccelY_mss();
-	acc.z = IMU.getAccelZ_mss();
-
-	return rates != lastRates;
 }
 
 void calibrateGyro() {
+	const int samples = 1000;
 	Serial.println("Calibrating gyro, stand still");
-	int status = IMU.calibrateGyro();
+	IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode
-	Serial.printf("Calibration status: %d\n", status);
+
-	IMU.setSrd(0);
+	gyroBias = Vector(0, 0, 0);
+	for (int i = 0; i < samples; i++) {
+		IMU.waitForData();
+		IMU.getGyro(rates.x, rates.y, rates.z);
+		gyroBias = gyroBias + rates;
+	}
+	gyroBias = gyroBias / samples;
+
 	printIMUCal();
+	configureIMU();
 }
 
 void calibrateAccel() {
-	Serial.println("Cal accel: place level"); delay(3000);
+	Serial.println("Calibrating accelerometer");
-	IMU.calibrateAccel();
+	IMU.setAccelRange(IMU.ACCEL_RANGE_2G);
-	Serial.println("Cal accel: place nose up"); delay(3000);
+	IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ);
-	IMU.calibrateAccel();
+	IMU.setSrd(19);
-	Serial.println("Cal accel: place nose down"); delay(3000);
+	Serial.setTimeout(60000);
-	IMU.calibrateAccel();
+	Serial.print("Place level [enter] "); Serial.readStringUntil('\n');
-	Serial.println("Cal accel: place on right side"); delay(3000);
+	calibrateAccelOnce();
-	IMU.calibrateAccel();
+	Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n');
-	Serial.println("Cal accel: place on left side"); delay(3000);
+	calibrateAccelOnce();
-	IMU.calibrateAccel();
+	Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n');
-	Serial.println("Cal accel: upside down"); delay(3000);
+	calibrateAccelOnce();
-	IMU.calibrateAccel();
+	Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n');
+	calibrateAccelOnce();
+	Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n');
+	calibrateAccelOnce();
+	Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n');
+	calibrateAccelOnce();
 	printIMUCal();
+	IMU.setAccelRange(IMU.ACCEL_RANGE_16G);
+	IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);
+	IMU.setSrd(0);
 }
 
-void loadAccelCal() {
+void calibrateAccelOnce() {
-	// NOTE: this should be changed to the actual values
+	const int samples = 100;
-	IMU.setAccelCalX(-0.0048542023, 1.0008112192);
+	static Vector accMax(-INFINITY, -INFINITY, -INFINITY);
-	IMU.setAccelCalY(0.0521845818, 0.9985780716);
+	static Vector accMin(INFINITY, INFINITY, INFINITY);
-	IMU.setAccelCalZ(0.5754694939, 1.0045746565);
-}
 
-void loadGyroCal() {
+	// Compute the average of the accelerometer readings
-	// NOTE: this should be changed to the actual values
+	acc = Vector(0, 0, 0);
-	IMU.setGyroBiasX_rads(-0.0185128022);
+	for (int i = 0; i < samples; i++) {
-	IMU.setGyroBiasY_rads(-0.0262369743);
+		IMU.waitForData();
-	IMU.setGyroBiasZ_rads(0.0163032326);
+		Vector sample;
+		IMU.getAccel(sample.x, sample.y, sample.z);
+		acc = acc + sample;
+	}
+	acc = acc / samples;
+
+	// Update the maximum and minimum values
+	if (acc.x > accMax.x) accMax.x = acc.x;
+	if (acc.y > accMax.y) accMax.y = acc.y;
+	if (acc.z > accMax.z) accMax.z = acc.z;
+	if (acc.x < accMin.x) accMin.x = acc.x;
+	if (acc.y < accMin.y) accMin.y = acc.y;
+	if (acc.z < accMin.z) accMin.z = acc.z;
+	Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z);
+	Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
+	Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z);
+	// Compute scale and bias
+	accScale = (accMax - accMin) / 2 / ONE_G;
+	accBias = (accMax + accMin) / 2;
 }
 
 void printIMUCal() {
-	Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());
+	Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
-	Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss());
+	Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
-	Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ());
+	Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
 }

flix/vector.h

@@ -44,6 +44,16 @@ public:
 		return Vector(x - b.x, y - b.y, z - b.z);
 	}
 
+	// Element-wise multiplication
+	Vector operator * (const Vector& b) const {
+		return Vector(x * b.x, y * b.y, z * b.z);
+	}
+
+	// Element-wise division
+	Vector operator / (const Vector& b) const {
+		return Vector(x / b.x, y / b.y, z / b.z);
+	}
+
 	inline bool operator == (const Vector& b) const {
 		return x == b.x && y == b.y && z == b.z;
 	}