diff --git a/right/src/timer.c b/right/src/timer.c
index 4106877..afa0ae0 100644
--- a/right/src/timer.c
+++ b/right/src/timer.c
@@ -1,12 +1,15 @@
 #include "fsl_pit.h"
 #include "timer.h"
+#include "peripherals/test_led.h"
 
 static volatile uint32_t CurrentTime;
+static uint32_t timerClockFrequency;
 
 void PIT_TIMER_HANDLER(void)
 {
     CurrentTime++;
-    PIT_ClearStatusFlags(PIT, PIT_TIMER_CHANNEL, PIT_TFLG_TIF_MASK);
+    //TEST_LED_TOGGLE();
+    PIT_ClearStatusFlags(PIT, PIT_TIMER_CHANNEL, kPIT_TimerFlag);
 }
 
 void Timer_Init(void)
@@ -15,9 +18,8 @@ void Timer_Init(void)
     PIT_GetDefaultConfig(&pitConfig);
     PIT_Init(PIT, &pitConfig);
 
-    // TODO: Why the interval needs to be multiplied by two to arrive to the correct timing?
-    // Figure it out and clean this up.
-    PIT_SetTimerPeriod(PIT, PIT_TIMER_CHANNEL, MSEC_TO_COUNT(TIMER_INTERVAL_MSEC*2, PIT_SOURCE_CLOCK));
+    timerClockFrequency = PIT_SOURCE_CLOCK;
+    PIT_SetTimerPeriod(PIT, PIT_TIMER_CHANNEL, MSEC_TO_COUNT(TIMER_INTERVAL_MSEC, timerClockFrequency));
 
     PIT_EnableInterrupts(PIT, PIT_TIMER_CHANNEL, kPIT_TimerInterruptEnable);
     EnableIRQ(PIT_TIMER_IRQ_ID);
@@ -28,20 +30,49 @@ uint32_t Timer_GetCurrentTime() {
     return CurrentTime;
 }
 
+uint32_t Timer_GetCurrentTimeMicros() {
+    uint32_t primask, count, ms;
+    primask = DisableGlobalIRQ(); // Make sure the read is atomic
+    count = PIT_GetCurrentTimerCount(PIT, PIT_TIMER_CHANNEL); // Read the current timer count
+    ms = CurrentTime; // Read the overflow counter
+    EnableGlobalIRQ(primask); // Enable interrupts again if they where enabled before - this should make it interrupt safe
+
+    uint32_t us = COUNT_TO_USEC(count, timerClockFrequency);
+    return ms * 1000U * TIMER_INTERVAL_MSEC + us;
+}
+
 void Timer_SetCurrentTime(uint32_t *time)
 {
-    *time = CurrentTime;
+    *time = Timer_GetCurrentTime();
+}
+
+void Timer_SetCurrentTimeMicros(uint32_t *time)
+{
+    *time = Timer_GetCurrentTimeMicros();
 }
 
 uint32_t Timer_GetElapsedTime(uint32_t *time)
 {
-    uint32_t elapsedTime = CurrentTime - *time;
+    uint32_t elapsedTime = Timer_GetCurrentTime() - *time;
+    return elapsedTime;
+}
+
+uint32_t Timer_GetElapsedTimeMicros(uint32_t *time)
+{
+    uint32_t elapsedTime = Timer_GetCurrentTimeMicros() - *time;
     return elapsedTime;
 }
 
 uint32_t Timer_GetElapsedTimeAndSetCurrent(uint32_t *time)
 {
     uint32_t elapsedTime = Timer_GetElapsedTime(time);
-    *time = CurrentTime;
+    *time = Timer_GetCurrentTime();
+    return elapsedTime;
+}
+
+uint32_t Timer_GetElapsedTimeAndSetCurrentMicros(uint32_t *time)
+{
+    uint32_t elapsedTime = Timer_GetElapsedTimeMicros(time);
+    *time = Timer_GetCurrentTimeMicros();
     return elapsedTime;
 }
diff --git a/right/src/timer.h b/right/src/timer.h
index 1ce3218..5acbcce 100644
--- a/right/src/timer.h
+++ b/right/src/timer.h
@@ -13,8 +13,12 @@
 
     void Timer_Init(void);
     uint32_t Timer_GetCurrentTime();
+    uint32_t Timer_GetCurrentTimeMicros();
     void Timer_SetCurrentTime(uint32_t *time);
+    void Timer_SetCurrentTimeMicros(uint32_t *time);
     uint32_t Timer_GetElapsedTime(uint32_t *time);
+    uint32_t Timer_GetElapsedTimeMicros(uint32_t *time);
     uint32_t Timer_GetElapsedTimeAndSetCurrent(uint32_t *time);
+    uint32_t Timer_GetElapsedTimeAndSetCurrentMicros(uint32_t *time);
 
 #endif