aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-01-11 13:36:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add level calibration

Browse Source
This commit is contained in:
Oleg Kalachev
2025-12-26 11:29:12 +03:00
parent 13a7e67b92
commit 0908674a60
3 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
flix/cli.ino
View File

@@ -42,6 +42,7 @@ const char* motd =
"log [dump] - print log header [and data]\n" "log [dump] - print log header [and data]\n"
"cr - calibrate RC\n" "cr - calibrate RC\n"
"ca - calibrate accel\n" "ca - calibrate accel\n"
"cl - calibrate level\n"
"mfr, mfl, mrr, mrl - test motor (remove props)\n" "mfr, mfl, mrr, mrl - test motor (remove props)\n"
"sys - show system info\n" "sys - show system info\n"
"reset - reset drone's state\n" "reset - reset drone's state\n"
@@ -148,6 +149,8 @@ void doCommand(String str, bool echo = false) {
calibrateRC(); calibrateRC();
} else if (command == "ca") { } else if (command == "ca") {
calibrateAccel(); calibrateAccel();
} else if (command == "cl") {
calibrateLevel();
} else if (command == "mfr") { } else if (command == "mfr") {
testMotor(MOTOR_FRONT_RIGHT); testMotor(MOTOR_FRONT_RIGHT);
} else if (command == "mfl") { } else if (command == "mfl") {

9
flix/imu.ino
View File

@@ -44,7 +44,6 @@ void readIMU() {
gyro = Quaternion::rotateVector(gyro, rotation.inversed()); gyro = Quaternion::rotateVector(gyro, rotation.inversed());
} }
void calibrateGyroOnce() { void calibrateGyroOnce() {
static Delay landedDelay(2); static Delay landedDelay(2);
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
@@ -108,6 +107,14 @@ void calibrateAccelOnce() {
accBias = (accMax + accMin) / 2; accBias = (accMax + accMin) / 2;
} }
void calibrateLevel() {
print("Place perfectly level [1 sec]\n");
pause(1);
Quaternion correction = Quaternion::fromBetweenVectors(Quaternion::rotateVector(Vector(0, 0, 1), attitude), Vector(0, 0, 1));
imuRotation = Quaternion::rotate(correction, Quaternion::fromEuler(imuRotation)).toEuler();
print("✓ Done: %.3f %.3f %.3f\n", degrees(imuRotation.x), degrees(imuRotation.y), degrees(imuRotation.z));
}
void printIMUCalibration() { void printIMUCalibration() {
print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z); print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z); print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);

1
gazebo/flix.h
View File

@@ -72,6 +72,7 @@ void resetParameters();
void setLED(bool on) {}; void setLED(bool on) {};
void calibrateGyro() { print("Skip gyro calibrating\n"); }; void calibrateGyro() { print("Skip gyro calibrating\n"); };
void calibrateAccel() { print("Skip accel calibrating\n"); }; void calibrateAccel() { print("Skip accel calibrating\n"); };
void calibrateLevel() { print("Skip level calibrating\n"); };
void printIMUCalibration() { print("cal: N/A\n"); }; void printIMUCalibration() { print("cal: N/A\n"); };
void printIMUInfo() {}; void printIMUInfo() {};
void printWiFiInfo() {}; void printWiFiInfo() {};