diff --git a/flix/cli.ino b/flix/cli.ino index b92f56d..2082eca 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -42,6 +42,7 @@ const char* motd = "log [dump] - print log header [and data]\n" "cr - calibrate RC\n" "ca - calibrate accel\n" +"cl - calibrate level\n" "mfr, mfl, mrr, mrl - test motor (remove props)\n" "sys - show system info\n" "reset - reset drone's state\n" @@ -148,6 +149,8 @@ void doCommand(String str, bool echo = false) { calibrateRC(); } else if (command == "ca") { calibrateAccel(); + } else if (command == "cl") { + calibrateLevel(); } else if (command == "mfr") { testMotor(MOTOR_FRONT_RIGHT); } else if (command == "mfl") { diff --git a/flix/imu.ino b/flix/imu.ino index 44a15fe..7d71b79 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -44,7 +44,6 @@ void readIMU() { gyro = Quaternion::rotateVector(gyro, rotation.inversed()); } - void calibrateGyroOnce() { static Delay landedDelay(2); if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary @@ -108,6 +107,14 @@ void calibrateAccelOnce() { 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() { 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); diff --git a/gazebo/flix.h b/gazebo/flix.h index e7d3a2c..78b38f3 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -72,6 +72,7 @@ void resetParameters(); void setLED(bool on) {}; void calibrateGyro() { print("Skip gyro calibrating\n"); }; void calibrateAccel() { print("Skip accel calibrating\n"); }; +void calibrateLevel() { print("Skip level calibrating\n"); }; void printIMUCalibration() { print("cal: N/A\n"); }; void printIMUInfo() {}; void printWiFiInfo() {};