Continuous gyro bias estimation (#17)

Estimate gyro bias continuously instead of calibrating the gyro at startup.

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

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

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;
 

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

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

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]},

gazebo/flix.h

@@ -21,6 +21,7 @@ Vector acc;
 Vector gyro;
 Vector rates;
 Quaternion attitude;
+bool landed;
 
 // declarations
 void step();