From 66a43ab2466364a1b28419857e16c8351e9f4884 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sat, 29 Mar 2025 12:21:40 +0300
Subject: [PATCH]  Continuous gyro bias estimation (#17)

Estimate gyro bias continuously instead of calibrating the gyro at startup.
---
 docs/troubleshooting.md |  1 -
 flix/cli.ino            |  3 ---
 flix/estimate.ino       |  2 +-
 flix/flix.ino           |  1 +
 flix/imu.ino            | 24 ++++++++----------------
 flix/parameters.ino     |  3 ---
 gazebo/flix.h           |  1 +
 7 files changed, 11 insertions(+), 24 deletions(-)

diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md
index c95dca9..c48cf15 100644
--- a/docs/troubleshooting.md
+++ b/docs/troubleshooting.md
@@ -16,7 +16,6 @@ Do the following:
 * **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file.
 * **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
-* **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while.
 * **Check the IMU is working**. Perform `imu` command and check its output:
   * The `status` field should be `OK`.
   * The `rate` field should be about 1000 (Hz).
diff --git a/flix/cli.ino b/flix/cli.ino
index 0143921..3997854 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -34,7 +34,6 @@ const char* motd =
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
 "cr - calibrate RC\n"
-"cg - calibrate gyro\n"
 "ca - calibrate accel\n"
 "mfr, mfl, mrr, mrl - test motor (remove props)\n"
 "reset - reset drone's state\n"
@@ -124,8 +123,6 @@ void doCommand(String str, bool echo = false) {
 		dumpLog();
 	} else if (command == "cr") {
 		calibrateRC();
-	} else if (command == "cg") {
-		calibrateGyro();
 	} else if (command == "ca") {
 		calibrateAccel();
 	} else if (command == "mfr") {
diff --git a/flix/estimate.ino b/flix/estimate.ino
index 8bcdf97..3d947d2 100644
--- a/flix/estimate.ino
+++ b/flix/estimate.ino
@@ -29,7 +29,7 @@ void applyGyro() {
 void applyAcc() {
 	// test should we apply accelerometer gravity correction
 	float accNorm = acc.norm();
-	bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
+	landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
 
 	if (!landed) return;
 
diff --git a/flix/flix.ino b/flix/flix.ino
index 2d89177..0c7ea7b 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -18,6 +18,7 @@ Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s
 Quaternion attitude; // estimated attitude
+bool landed; // are we landed and stationary
 float motors[4]; // normalized motors thrust in range [-1..1]
 
 void setup() {
diff --git a/flix/imu.ino b/flix/imu.ino
index 25167b8..00ebfb4 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -5,6 +5,7 @@
 
 #include <SPI.h>
 #include <MPU9250.h>
+#include "lpf.h"
 #include "util.h"
 
 MPU9250 IMU(SPI);
@@ -17,8 +18,6 @@ void setupIMU() {
 	print("Setup IMU\n");
 	IMU.begin();
 	configureIMU();
-	delay(500); // wait a bit before calibrating
-	calibrateGyro();
 }
 
 void configureIMU() {
@@ -32,6 +31,7 @@ void readIMU() {
 	IMU.waitForData();
 	IMU.getGyro(gyro.x, gyro.y, gyro.z);
 	IMU.getAccel(acc.x, acc.y, acc.z);
+	calibrateGyroOnce();
 	// apply scale and bias
 	acc = (acc - accBias) / accScale;
 	gyro = gyro - gyroBias;
@@ -47,21 +47,13 @@ void rotateIMU(Vector& data) {
 	// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation
 }
 
-void calibrateGyro() {
-	const int samples = 1000;
-	print("Calibrating gyro, stand still\n");
-	IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode
+void calibrateGyroOnce() {
+	static float landedTime = 0;
+	landedTime = landed ? landedTime + dt : 0;
+	if (landedTime < 2) return; // calibrate only if definitely stationary
 
-	gyroBias = Vector(0, 0, 0);
-	for (int i = 0; i < samples; i++) {
-		IMU.waitForData();
-		IMU.getGyro(gyro.x, gyro.y, gyro.z);
-		gyroBias = gyroBias + gyro;
-	}
-	gyroBias = gyroBias / samples;
-
-	printIMUCal();
-	configureIMU();
+	static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
+	gyroBias = gyroCalibrationFilter.update(gyro);
 }
 
 void calibrateAccel() {
diff --git a/flix/parameters.ino b/flix/parameters.ino
index 4447219..612fe34 100644
--- a/flix/parameters.ino
+++ b/flix/parameters.ino
@@ -48,9 +48,6 @@ Parameter parameters[] = {
 	{"ACC_SCALE_X", &accScale.x},
 	{"ACC_SCALE_Y", &accScale.y},
 	{"ACC_SCALE_Z", &accScale.z},
-	{"GYRO_BIAS_X", &gyroBias.x},
-	{"GYRO_BIAS_Y", &gyroBias.y},
-	{"GYRO_BIAS_Z", &gyroBias.z},
 	// rc
 	{"RC_NEUTRAL_0", &channelNeutral[0]},
 	{"RC_NEUTRAL_1", &channelNeutral[1]},
diff --git a/gazebo/flix.h b/gazebo/flix.h
index bcda0c3..a4866a0 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -21,6 +21,7 @@ Vector acc;
 Vector gyro;
 Vector rates;
 Quaternion attitude;
+bool landed;
 
 // declarations
 void step();