Add level calibration

Which seems a better name

.markdownlint.json

@@ -65,5 +65,6 @@
 			"PX4"
 		]
 	},
-	"MD045": false
+	"MD045": false,
+	"MD060": false
 }

docs/usage.md

@@ -73,14 +73,6 @@ ICM20948 imu(SPI);  // For ICM-20948
 MPU6050 imu(Wire);  // For MPU-6050
 ```
 
-### Setup the IMU orientation
-
-The IMU orientation is defined in `rotateIMU` function in the `imu.ino` file. Change it so it converts the IMU axes to the drone's axes correctly. **Drone axes are X forward, Y left, Z up**:
-
-<img src="img/drone-axes.svg" width="200">
-
-See various [IMU boards axes orientations table](https://github.com/okalachev/flixperiph/?tab=readme-ov-file#imu-axes-orientation) to help you set up the correct orientation.
-
 ### Connect using QGroundControl
 
 QGroundControl is a ground control station software that can be used to monitor and control the drone.
@@ -104,11 +96,35 @@ To access the console using QGroundControl:
 
 1. Connect to the drone using QGroundControl app.
 2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
-   <img src="img/cli.png" width="400">
+
+<img src="img/cli.png" width="400">
 
 > [!TIP]
 > Use `help` command to see the list of available commands.
 
+### Access parameters
+
+The drone is configured using parameters. To access and modify them, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*:
+
+<img src="img/parameters.png" width="400">
+
+### Define IMU orientation
+
+Use parameters, to define the IMU board axes orientation relative to the drone's axes: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
+
+The drone has *X* axis pointing forward, *Y* axis pointing left, and *Z* axis pointing up, and the supported IMU boards have *X* axis pointing to the pins side and *Z* axis pointing up from the side with the components:
+
+<img src="img/imu-axes.png" width="200">
+
+Use the following table to set the parameters for common IMU orientations:
+
+|Orientation|Parameters|Orientation|Parameters|
+|:-:|-|-|-|
+|<img src="img/imu-rot-1.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0    |<img src="img/imu-rot-5.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
+|<img src="img/imu-rot-2.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-6.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
+|<img src="img/imu-rot-3.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-7.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
+|<img src="img/imu-rot-4.png" width="200"><br>☑️ **Default**|<br>`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|<img src="img/imu-rot-8.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
+
 ### Calibrate accelerometer
 
 Before flight you need to calibrate the accelerometer:
@@ -221,10 +237,6 @@ In this mode, the pilot inputs are ignored (except the mode switch, if configure
 
 If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
 
-## Adjusting parameters
-
-You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*.
-
 <img src="img/parameters.png" width="400">
 
 ## Flight log

flix/cli.ino

@@ -42,6 +42,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"
@@ -148,6 +149,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

@@ -8,8 +8,8 @@
 #include "lpf.h"
 #include "util.h"
 
-#define WEIGHT_ACC 0.003
-#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+float accWeight = 0.003;
+LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
 
 void estimate() {
 	applyGyro();
@@ -18,7 +18,6 @@ void estimate() {
 
 void applyGyro() {
 	// filter gyro to get angular rates
-	static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude
@@ -34,7 +33,7 @@ void applyAcc() {
 
 	// calculate accelerometer correction
 	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
-	Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
+	Vector correction = Vector::rotationVectorBetween(acc, up) * accWeight;
 
 	// apply correction
 	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));

flix/imu.ino

@@ -10,6 +10,7 @@
 #include "util.h"
 
 MPU9250 imu(SPI);
+Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles
 
 Vector accBias;
 Vector accScale(1, 1, 1);
@@ -37,24 +38,18 @@ void readIMU() {
 	// apply scale and bias
 	acc = (acc - accBias) / accScale;
 	gyro = gyro - gyroBias;
-	// rotate
-	rotateIMU(acc);
-	rotateIMU(gyro);
-}
-
-void rotateIMU(Vector& data) {
-	// Rotate from LFD to FLU
-	// NOTE: In case of using other IMU orientation, change this line:
-	data = Vector(data.y, data.x, -data.z);
-	// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation
+	// rotate to body frame
+	Quaternion rotation = Quaternion::fromEuler(imuRotation);
+	acc = Quaternion::rotateVector(acc, rotation.inversed());
+	gyro = Quaternion::rotateVector(gyro, rotation.inversed());
 }
 
 void calibrateGyroOnce() {
 	static Delay landedDelay(2);
 	if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
 
-	static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
-	gyroBias = gyroCalibrationFilter.update(gyro);
+	static LowPassFilter<Vector> gyroBiasFilter(0.001);
+	gyroBias = gyroBiasFilter.update(gyro);
 }
 
 void calibrateAccel() {
@@ -112,6 +107,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/parameters.ino

@@ -43,12 +43,18 @@ Parameter parameters[] = {
 	{"CTL_Y_RATE_MAX", &maxRate.z},
 	{"CTL_TILT_MAX", &tiltMax},
 	// imu
+	{"IMU_ROT_ROLL", &imuRotation.x},
+	{"IMU_ROT_PITCH", &imuRotation.y},
+	{"IMU_ROT_YAW", &imuRotation.z},
 	{"IMU_ACC_BIAS_X", &accBias.x},
 	{"IMU_ACC_BIAS_Y", &accBias.y},
 	{"IMU_ACC_BIAS_Z", &accBias.z},
 	{"IMU_ACC_SCALE_X", &accScale.x},
 	{"IMU_ACC_SCALE_Y", &accScale.y},
 	{"IMU_ACC_SCALE_Z", &accScale.z},
+	// estimate
+	{"EST_ACC_WEIGHT", &accWeight},
+	{"EST_RATES_LPF_A", &ratesFilter.alpha},
 	// rc
 	{"RC_ZERO_0", &channelZero[0]},
 	{"RC_ZERO_1", &channelZero[1]},

gazebo/flix.h

@@ -22,6 +22,7 @@ Vector gyro;
 Vector rates;
 Quaternion attitude;
 bool landed;
+Vector imuRotation;
 
 // declarations
 void step();
@@ -71,6 +72,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() {};