aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 01:29:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Calibrate gyro continuously when landed and stationary

Browse Source
This commit is contained in:
Oleg Kalachev 2024-12-24 22:19:54 +03:00
parent fd30027ea4
commit 073c860b90
4 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flix/estimate.ino
View File

@ -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

1
flix/flix.ino
View File

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

10
flix/imu.ino
View File

@ -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");

6
flix/parameters.ino
View File

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