diff --git a/utility/MRAA/compatibility.cpp b/utility/MRAA/compatibility.cpp
index 8593fbd0..efed1e7b 100644
--- a/utility/MRAA/compatibility.cpp
+++ b/utility/MRAA/compatibility.cpp
@@ -1,7 +1,6 @@
 #include "compatibility.h"
+#include <time.h>
 #include <chrono>
-//static struct timeval start, end;
-//static long mtime, seconds, useconds;
 
 /**********************************************************************/
 /**
@@ -10,33 +9,30 @@
  */
 void __msleep(int milisec)
 {
-    struct timespec req = {0};
-    req.tv_sec = 0;
-    req.tv_nsec = milisec * 1000000L;
-    nanosleep(&req, (struct timespec*)NULL);
-    //usleep(milisec*1000);
+    struct timespec req; // = {0};
+    req.tv_sec = (time_t)milisec / 1000;
+    req.tv_nsec = (milisec % 1000) * 1000000L;
+    clock_nanosleep(CLOCK_REALTIME, 0, &req, NULL);
 }
 
-void __usleep(int milisec)
+void __usleep(int microsec)
 {
-    struct timespec req = {0};
-    req.tv_sec = 0;
-    req.tv_nsec = milisec * 1000L;
-    nanosleep(&req, (struct timespec*)NULL);
-    //usleep(milisec);
+    struct timespec req; // = {0};
+    req.tv_sec = (time_t)microsec / 1000000;
+    req.tv_nsec = (microsec / 1000000) * 1000;
+    clock_nanosleep(CLOCK_REALTIME, 0, &req, NULL);
 }
 
-auto start = std::chrono::steady_clock::now();
-
 /**
  * This function is added in order to simulate arduino millis() function
  */
 void __start_timer()
 {
-    //gettimeofday(&start, NULL);
 }
 
-long __millis()
+auto start = std::chrono::steady_clock::now();
+
+uint32_t __millis()
 {
     auto end = std::chrono::steady_clock::now();
 
diff --git a/utility/MRAA/compatibility.h b/utility/MRAA/compatibility.h
index ffc6953f..8e55ae8b 100644
--- a/utility/MRAA/compatibility.h
+++ b/utility/MRAA/compatibility.h
@@ -15,6 +15,7 @@ extern "C" {
 #include <stddef.h>
 #include <time.h>
 #include <sys/time.h>
+#include <stdint.h>
 
 void __msleep(int milisec);
 
@@ -22,7 +23,7 @@ void __usleep(int milisec);
 
 void __start_timer();
 
-long __millis();
+uint32_t __millis();
 
 #ifdef __cplusplus
 }