1 files changed, 11 insertions, 6 deletions
diff --git a/main.cpp b/main.cpp
index 14f8a8a..27fab50 100644
--- a/main.cpp
+++ b/main.cpp
@@ -12,11 +12,14 @@
 class LEDThread : public BaseThread<LEDThread, 128> {
 	public:
 		noreturn_t thread_main() {
-			while (1) {
+			systime_t time = chTimeNow();     // T0
+			while (TRUE) {
+				time += MS2ST(1000);            // Next deadline
 				palClearPad(GPIOA, 5);
-				chThdSleepMilliseconds(500);
+				chThdSleepUntil(time);
+				time += MS2ST(1000);            // Next deadline
 				palSetPad(GPIOA, 5);
-				chThdSleepMilliseconds(500);
+				chThdSleepUntil(time);
 			}
 		}
 };
@@ -84,9 +87,8 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 			acc.init();
 			magn.init();
 			
-			systime_t nexttime = chTimeNow();
+			systime_t time = chTimeNow();
 			while (1) {
-				nexttime += MS2ST(100);
 				gyro.update();
 				acc.update();
 				magn.update();
@@ -126,7 +128,10 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
 					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
 				
-				//chThdSleepUntil(nexttime);
+				time += MS2ST(10);
+				if(time > chTimeNow()) {
+					chThdSleepUntil(time);
+				}
 			}
 		}
 };