diff --git a/flix/estimate.ino b/flix/estimate.ino index b0fae96..db34a3f 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -31,6 +31,7 @@ void applyAcc() { float accNorm = acc.norm(); bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f; + setLED(landed); if (!landed) return; // calculate accelerometer correction diff --git a/flix/flix.ino b/flix/flix.ino index c5de571..6a950f7 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -35,6 +35,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() { diff --git a/flix/imu.ino b/flix/imu.ino index 46c526a..caa8b79 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -22,7 +22,7 @@ void setupIMU() { } } configureIMU(); - calibrateGyro(); + // calibrateGyro(); } void configureIMU() { @@ -36,6 +36,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; @@ -51,6 +52,13 @@ void rotateIMU(Vector& data) { // Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation } +void calibrateGyroOnce() { + if (!landed) return; + static float samples = 0; // overflows after 49 days at 1000 Hz + samples++; + gyroBias = gyroBias + (gyro - gyroBias) / samples; // running average +} + void calibrateGyro() { const int samples = 1000; Serial.println("Calibrating gyro, stand still"); diff --git a/flix/parameters.ino b/flix/parameters.ino index 6b9e688..e66b365 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -41,9 +41,9 @@ 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}, + // {"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]},