From f8f746b0cdbc7ad51183c1c7f8719caaaefed09c Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 30 Jan 2026 07:49:26 +0300
Subject: [PATCH] Add level calibration

---
 flix/cli.ino  | 3 +++
 flix/imu.ino  | 8 ++++++++
 gazebo/flix.h | 1 +
 3 files changed, 12 insertions(+)

diff --git a/flix/cli.ino b/flix/cli.ino
index 3250d0e..e80d4e4 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -46,6 +46,7 @@ const char* motd =
 "log [dump] - print log header [and data]\n"
 "cr - calibrate RC\n"
 "ca - calibrate accel\n"
+"cl - calibrate level\n"
 "mfr, mfl, mrr, mrl - test motor (remove props)\n"
 "sys - show system info\n"
 "reset - reset drone's state\n"
@@ -151,6 +152,8 @@ void doCommand(String str, bool echo = false) {
 		calibrateRC();
 	} else if (command == "ca") {
 		calibrateAccel();
+	} else if (command == "cl") {
+		calibrateLevel();
 	} else if (command == "mfr") {
 		testMotor(MOTOR_FRONT_RIGHT);
 	} else if (command == "mfl") {
diff --git a/flix/imu.ino b/flix/imu.ino
index ffdc1ff..66c2a7e 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -110,6 +110,14 @@ void calibrateAccelOnce() {
 	accBias = (accMax + accMin) / 2;
 }
 
+void calibrateLevel() {
+	print("Place perfectly level [1 sec]\n");
+	pause(1);
+	Quaternion correction = Quaternion::fromBetweenVectors(Quaternion::rotateVector(Vector(0, 0, 1), attitude), Vector(0, 0, 1));
+	imuRotation = Quaternion::rotate(correction, Quaternion::fromEuler(imuRotation)).toEuler();
+	print("✓ Done: %.3f %.3f %.3f\n", degrees(imuRotation.x), degrees(imuRotation.y), degrees(imuRotation.z));
+}
+
 void printIMUCalibration() {
 	print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
 	print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 5c61ee5..43ea0a2 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -71,6 +71,7 @@ void resetParameters();
 void setLED(bool on) {};
 void calibrateGyro() { print("Skip gyro calibrating\n"); };
 void calibrateAccel() { print("Skip accel calibrating\n"); };
+void calibrateLevel() { print("Skip level calibrating\n"); };
 void printIMUCalibration() { print("cal: N/A\n"); };
 void printIMUInfo() {};
 void printWiFiInfo() {};