Use precise time intervals.

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);

+				}

 			}

 		}

 };