diff --git a/flix/cli.ino b/flix/cli.ino index b92f56d..df81b39 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" diff --git a/flix/imu.ino b/flix/imu.ino index 44a15fe..4bfd74b 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -44,6 +44,13 @@ void readIMU() { gyro = Quaternion::rotateVector(gyro, rotation.inversed()); } +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 calibrateGyroOnce() { static Delay landedDelay(2);