Add level calibration

docs/usage.md

@@ -148,29 +148,27 @@ Reboot the drone to apply the changes.
 > [!CAUTION]
 > **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
 
-### Important: check everything works
+### Check everything works
 
-1. Check the IMU is working: perform `imu` command in the console and check the output:
+1. 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).
    * The `accel` and `gyro` fields should change as you move the drone.
-   * The `accel bias` and `accel scale` fields should contain calibration parameters (not zeros and ones).
-   * The `gyro bias` field should contain estimated gyro bias (not zeros).
    * The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
 
 2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
 
     <a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
 
-3. Perform motor tests. Use the following commands **— remove the propellers before running the tests!**
+3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
 
-   * `mfr` — rotate front right motor (counter-clockwise).
-   * `mfl` — rotate front left motor (clockwise).
-   * `mrl` — rotate rear left motor (counter-clockwise).
-   * `mrr` — rotate rear right motor (clockwise).
+   * `mfr` — should rotate front right motor (counter-clockwise).
+   * `mfl` — should rotate front left motor (clockwise).
+   * `mrl` — should rotate rear left motor (counter-clockwise).
+   * `mrr` — should rotate rear right motor (clockwise).
 
-   Make sure rotation directions and propeller types match the following diagram:
+   Rotation diagram:
 
    <img src="img/motors.svg" width=200>
 
@@ -236,11 +234,11 @@ When finished flying, **disarm** the drone, moving the left stick to the bottom
 
 ### Flight modes
 
-Flight mode is changed using mode switch on the remote control (if configured) or using the console commands. The main flight mode is *STAB*.
+Flight mode is changed using mode switch on the remote control or using the command line.
 
 #### STAB
 
-In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
+The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
 
 > [!IMPORTANT]
 > The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
@@ -255,7 +253,7 @@ In this mode, the pilot controls the angular rates. This control method is diffi
 
 #### AUTO
 
-In this mode, the pilot inputs are ignored (except the mode switch). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed behavior.
+In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
 
 If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
 

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

@@ -6,18 +6,18 @@
 #include <Preferences.h>
 #include "util.h"
 
-extern int channelZero[16];
-extern int channelMax[16];
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern float channelZero[16];
+extern float channelMax[16];
+extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int wifiMode, udpLocalPort, udpRemotePort;
 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) {

flix/rc.ino

@@ -9,14 +9,15 @@
 SBUS rc(Serial2);
 
 uint16_t channels[16]; // raw rc channels
-int channelZero[16]; // calibration zero values
-int channelMax[16]; // calibration max values
+float channelZero[16]; // calibration zero values
+float channelMax[16]; // calibration max values
 
 float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
 float controlMode = NAN;
 float controlTime = NAN; // time of the last controls update
 
-int rollChannel = -1, pitchChannel = -1, throttleChannel = -1, yawChannel = -1, modeChannel = -1; // channel mapping
+// Channels mapping (nan means not assigned):
+float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
 
 void setupRC() {
 	print("Setup RC\n");
@@ -40,11 +41,11 @@ void normalizeRC() {
 		controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
 	}
 	// Update control values
-	controlRoll = rollChannel < 0 ? 0 : controls[rollChannel];
-	controlPitch = pitchChannel < 0 ? 0 : controls[pitchChannel];
-	controlYaw = yawChannel < 0 ? 0 : controls[yawChannel];
-	controlThrottle = throttleChannel < 0 ? 0 : controls[throttleChannel];
-	controlMode = modeChannel < 0 ? NAN : controls[modeChannel]; // mode control is ineffective if not mapped
+	controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
+	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
+	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
+	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
+	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
 }
 
 void calibrateRC() {
@@ -63,7 +64,7 @@ void calibrateRC() {
 	printRCCalibration();
 }
 
-void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const char *str) {
+void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
 	print("%s", str);
 	pause(3);
 	for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
@@ -84,15 +85,15 @@ void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const c
 		channelZero[ch] = in[ch];
 		channelMax[ch] = out[ch];
 	} else {
-		*channel = -1;
+		*channel = NAN;
 	}
 }
 
 void printRCCalibration() {
 	print("Control   Ch     Zero   Max\n");
-	print("Roll      %-7d%-7d%-7d\n", rollChannel, rollChannel < 0 ? 0 : channelZero[rollChannel], rollChannel < 0 ? 0 : channelMax[rollChannel]);
-	print("Pitch     %-7d%-7d%-7d\n", pitchChannel, pitchChannel < 0 ? 0 : channelZero[pitchChannel], pitchChannel < 0 ? 0 : channelMax[pitchChannel]);
-	print("Yaw       %-7d%-7d%-7d\n", yawChannel, yawChannel < 0 ? 0 : channelZero[yawChannel], yawChannel < 0 ? 0 : channelMax[yawChannel]);
-	print("Throttle  %-7d%-7d%-7d\n", throttleChannel, throttleChannel < 0 ? 0 : channelZero[throttleChannel], throttleChannel < 0 ? 0 : channelMax[throttleChannel]);
-	print("Mode      %-7d%-7d%-7d\n", modeChannel, modeChannel < 0 ? 0 : channelZero[modeChannel], modeChannel < 0 ? 0 : channelMax[modeChannel]);
+	print("Roll      %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
+	print("Pitch     %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
+	print("Yaw       %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
+	print("Throttle  %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
+	print("Mode      %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
 }

gazebo/flix.h

@@ -43,7 +43,7 @@ void doCommand(String str, bool echo);
 void handleInput();
 void normalizeRC();
 void calibrateRC();
-void calibrateRCChannel(int *channel, uint16_t zero[16], uint16_t max[16], const char *str);
+void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
 void printRCCalibration();
 void printLogHeader();
 void printLogData();
@@ -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()