Add level calibration

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") {

flix/estimate.ino

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Attitude estimation using gyro and accelerometer
+// Attitude estimation from gyro and accelerometer
 
 #include "quaternion.h"
 #include "vector.h"

flix/flix.ino

@@ -21,8 +21,8 @@ void setup() {
 	disableBrownOut();
 	setupParameters();
 	setupLED();
-	setLED(true);
 	setupMotors();
+	setLED(true);
 	setupWiFi();
 	setupIMU();
 	setupRC();

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);

flix/mavlink.ino

@@ -8,13 +8,12 @@
 
 extern float controlTime;
 
+bool mavlinkConnected = false;
+String mavlinkPrintBuffer;
 int mavlinkSysId = 1;
 Rate telemetryFast(10);
 Rate telemetrySlow(2);
 
-bool mavlinkConnected = false;
-String mavlinkPrintBuffer;
-
 void processMavlink() {
 	sendMavlink();
 	receiveMavlink();
@@ -42,9 +41,9 @@ void sendMavlink() {
 	}
 
 	if (telemetryFast && mavlinkConnected) {
-		const float offset[] = {0, 0, 0, 0};
+		const float zeroQuat[] = {0, 0, 0, 0};
 		mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, offset); // convert to frd
+			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
 		sendMessage(&msg);
 
 		mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,

flix/parameters.ino

@@ -15,9 +15,9 @@ extern float rcLossTimeout, descendTime;
 Preferences storage;
 
 struct Parameter {
-	const char *name; // max length is 15
+	const char *name; // max length is 15 (Preferences key limit)
 	bool integer;
-	union { float *f; int *i; }; // pointer to the variable
+	union { float *f; int *i; }; // pointer to variable
 	float cache; // what's stored in flash
 	Parameter(const char *name, float *variable) : name(name), integer(false), f(variable) {};
 	Parameter(const char *name, int *variable) : name(name), integer(true), i(variable) {};
@@ -112,7 +112,6 @@ Parameter parameters[] = {
 };
 
 void setupParameters() {
-	print("Setup parameters\n");
 	storage.begin("flix", false);
 	// Read parameters from storage
 	for (auto &parameter : parameters) {

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() {};

tools/pyflix/flix.py

@@ -138,7 +138,7 @@ class Flix:
         while True:
             try:
                 msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
-                if msg is None or msg.get_srcSystem() != self.system_id:
+                if msg is None:
                     continue
                 self._connected()
                 msg_dict = msg.to_dict()