Continuous gyro bias estimation (#17)

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

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