aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-29 04:19:00 +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(); float accNorm = acc.norm();
bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f; bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
setLED(landed);
if (!landed) return; if (!landed) return;
// calculate accelerometer correction // 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 acc; // accelerometer data, m/s/s
Vector rates; // filtered angular rates, rad/s Vector rates; // filtered angular rates, rad/s
Quaternion attitude; // estimated attitude Quaternion attitude; // estimated attitude
bool landed; // are we landed and stationary
float motors[4]; // normalized motors thrust in range [-1..1] float motors[4]; // normalized motors thrust in range [-1..1]
void setup() { void setup() {

10
flix/imu.ino
View File

@ -22,7 +22,7 @@ void setupIMU() {
} }
} }
configureIMU(); configureIMU();
calibrateGyro(); // calibrateGyro();
} }
void configureIMU() { void configureIMU() {
@ -36,6 +36,7 @@ void readIMU() {
IMU.waitForData(); IMU.waitForData();
IMU.getGyro(gyro.x, gyro.y, gyro.z); IMU.getGyro(gyro.x, gyro.y, gyro.z);
IMU.getAccel(acc.x, acc.y, acc.z); IMU.getAccel(acc.x, acc.y, acc.z);
calibrateGyroOnce();
// apply scale and bias // apply scale and bias
acc = (acc - accBias) / accScale; acc = (acc - accBias) / accScale;
gyro = gyro - gyroBias; gyro = gyro - gyroBias;
@ -51,6 +52,13 @@ void rotateIMU(Vector& data) {
// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation // 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() { void calibrateGyro() {
const int samples = 1000; const int samples = 1000;
Serial.println("Calibrating gyro, stand still"); 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_X", &accScale.x},
{"ACC_SCALE_Y", &accScale.y}, {"ACC_SCALE_Y", &accScale.y},
{"ACC_SCALE_Z", &accScale.z}, {"ACC_SCALE_Z", &accScale.z},
{"GYRO_BIAS_X", &gyroBias.x}, // {"GYRO_BIAS_X", &gyroBias.x},
{"GYRO_BIAS_Y", &gyroBias.y}, // {"GYRO_BIAS_Y", &gyroBias.y},
{"GYRO_BIAS_Z", &gyroBias.z}, // {"GYRO_BIAS_Z", &gyroBias.z},
// rc // rc
{"RC_NEUTRAL_0", &channelNeutral[0]}, {"RC_NEUTRAL_0", &channelNeutral[0]},
{"RC_NEUTRAL_1", &channelNeutral[1]}, {"RC_NEUTRAL_1", &channelNeutral[1]},