diff --git a/src/entrypoints/imu-calibration.cpp b/src/entrypoints/imu-calibration.cpp index d42b4977fefe71c56cc4942a9242d967d4a18f93..b28705fef44f739ba0f05905581a4ad867135971 100644 --- a/src/entrypoints/imu-calibration.cpp +++ b/src/entrypoints/imu-calibration.cpp @@ -29,6 +29,7 @@ #include <iostream> using namespace Boardcore; +using namespace Boardcore::Constants; using namespace Eigen; using namespace miosix; @@ -108,14 +109,14 @@ void calibrateMagnetometer() scheduler.start(); // Wait and then stop the sampling - auto startTick = getTick(); - auto lastTick = startTick; - while (getTick() - startTick < MAG_CALIBRATION_DURATION * 1e3) + auto startTime = getTime(); + auto lastTime = startTime; + while (getTime() - startTime < MAG_CALIBRATION_DURATION * NS_IN_S) { mpu.sample(); calibration.feed(mpu.getLastSample()); - Thread::sleepUntil(lastTick + IMU_SAMPLE_PERIOD); - lastTick = getTick(); + Thread::nanoSleepUntil(lastTime + (IMU_SAMPLE_PERIOD * NS_IN_MS)); + lastTime = getTime(); } scheduler.stop();