diff --git a/flight/OpenPilot/System/inc/FreeRTOSConfig.h b/flight/OpenPilot/System/inc/FreeRTOSConfig.h
index f82ab3875..1262aed63 100644
--- a/flight/OpenPilot/System/inc/FreeRTOSConfig.h
+++ b/flight/OpenPilot/System/inc/FreeRTOSConfig.h
@@ -75,9 +75,9 @@ NVIC value of 255. */
 #if defined(DEBUG)
 #define configGENERATE_RUN_TIME_STATS 1
 #define INCLUDE_uxTaskGetRunTime 1
-#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()  
+#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() TaskTimerConfigure()
 // Note: Using the tick count defeats the purpose here, need some timer on the scale of 10khz
-#define portGET_RUN_TIME_COUNTER_VALUE()  xTaskGetTickCount()
+#define portGET_RUN_TIME_COUNTER_VALUE()  RTC_GetCounter()
 #endif
 
 
diff --git a/flight/OpenPilot/System/taskmonitor.c b/flight/OpenPilot/System/taskmonitor.c
index b4dc65a68..fd3512e9c 100644
--- a/flight/OpenPilot/System/taskmonitor.c
+++ b/flight/OpenPilot/System/taskmonitor.c
@@ -84,6 +84,27 @@ int32_t TaskMonitorRemove(TaskInfoRunningElem task)
 	}
 }
 
+/** 
+ * Temporarily put function here for configuring RTC for task timing
+ * Only used in debug mode
+ */
+void TaskTimerConfigure( void ) 
+{
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,
+			       ENABLE);
+	PWR_BackupAccessCmd(ENABLE);
+	
+	RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
+	RCC_RTCCLKCmd(ENABLE);
+	RTC_WaitForLastTask();
+	RTC_WaitForSynchro();
+	RTC_WaitForLastTask();
+	RTC_SetPrescaler(0);	// counting at 8e6 / 128
+	RTC_WaitForLastTask();
+	RTC_SetCounter(0);
+	RTC_WaitForLastTask();	
+}
+
 /**
  * Update the status of all tasks
  */
diff --git a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/GCC/ARM_CM3/portmacro.h b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/GCC/ARM_CM3/portmacro.h
index 3a106d568..223bf4f33 100755
--- a/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/GCC/ARM_CM3/portmacro.h
+++ b/flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/GCC/ARM_CM3/portmacro.h
@@ -55,6 +55,8 @@
 #ifndef PORTMACRO_H
 #define PORTMACRO_H
 
+#include "stdint.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -135,6 +137,9 @@ extern void vPortYieldFromISR( void );
 
 extern void vPortEnterCritical( void );
 extern void vPortExitCritical( void );
+	
+extern void TaskTimerConfigure( void );
+extern uint32_t RTC_GetCounter( void );
 
 #define portDISABLE_INTERRUPTS()	portSET_INTERRUPT_MASK()
 #define portENABLE_INTERRUPTS()		portCLEAR_INTERRUPT_MASK()