diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile
index 4177e277f..61484b39a 100644
--- a/flight/AHRS/Makefile
+++ b/flight/AHRS/Makefile
@@ -91,6 +91,7 @@ SRC = ahrs.c
 SRC += pios_board.c
 SRC += ahrs_adc.c
 SRC += ahrs_fsm.c
+SRC += ahrs_timer.c
 SRC += insgps.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
 
diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 9f8bd3612..a80fad927 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -35,6 +35,7 @@
 /* OpenPilot Includes */
 #include "ahrs.h"
 #include "ahrs_adc.h"
+#include "ahrs_timer.h"
 #include "pios_opahrs_proto.h"
 #include "ahrs_fsm.h"		/* lfsm_state */
 #include "insgps.h"
@@ -151,8 +152,12 @@ void process_spi_request(void);
 void downsample_data(void);
 void calibrate_sensors(void);
 void converge_insgps();
-void timer_start();
-uint32_t timer_count();
+
+volatile uint32_t last_counter_idle_start = 0;
+volatile uint32_t last_counter_idle_end = 0;
+volatile uint32_t idle_counts;
+volatile uint32_t running_counts;
+uint32_t counter_val;
 
 /**
  * @addtogroup AHRS_Global_Data AHRS Global Data
@@ -183,10 +188,8 @@ double BaseECEF[3] = {0, 0, 0};
 float Rne[3][3];
 //! Indicates the communications are requesting a calibration
 uint8_t calibration_pending = FALSE;
-//! Counter for tracking the idle level
-static uint32_t idle_counter = 0;
 //! The oversampling rate, ekf is 2k / this
-static uint8_t adc_oversampling = 18;
+static uint8_t adc_oversampling = 15;
 /**
  * @}
  */
@@ -282,9 +285,12 @@ int main()
         }
     }
 #endif
+	
+	timer_start();
     
     /******************* Main EKF loop ****************************/
     while (1) {
+
         // Alive signal
 		if((total_conversion_blocks % 100) == 0)
 			PIOS_LED_Toggle(LED1);
@@ -300,11 +306,16 @@ int main()
         PIOS_HMC5843_ReadMag(mag_data.raw.axis);
 #endif        
         // Delay for valid data
-        idle_counter = 0;
-        do {
-            idle_counter ++;
-            //process_spi_request();
-        } while ( ahrs_state != AHRS_DATA_READY );
+		
+		counter_val = timer_count();
+		running_counts = counter_val - last_counter_idle_end;
+		last_counter_idle_start = counter_val;
+
+        while ( ahrs_state != AHRS_DATA_READY );
+		
+		counter_val = timer_count();
+		idle_counts = counter_val - last_counter_idle_start;
+		last_counter_idle_end = counter_val;
         
         ahrs_state = AHRS_PROCESSING;
         
@@ -802,8 +813,7 @@ void process_spi_request(void)
             user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2];
             
             // compute the idle fraction
-            user_tx_v1.payload.user.v.rsp.update.load = ((float) ekf_too_slow / (float) total_conversion_blocks) * 100;
-			//(MAX_IDLE_COUNT - idle_counter) * 100 / MAX_IDLE_COUNT;
+            user_tx_v1.payload.user.v.rsp.update.load = ((float) running_counts / (float) (idle_counts+running_counts)) * 100;
             
             dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
             lfsm_user_set_tx_v1 (&user_tx_v1);
diff --git a/flight/AHRS/ahrs_timer.c b/flight/AHRS/ahrs_timer.c
new file mode 100644
index 000000000..76e18bc10
--- /dev/null
+++ b/flight/AHRS/ahrs_timer.c
@@ -0,0 +1,61 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS AHRS Control
+ * @brief The AHRS Modules perform
+ *
+ * @{ 
+ * @addtogroup AHRS_TIMER
+ * @brief Sets up a simple timer that can be polled to estimate idle time
+ * @{ 
+ *
+ *
+ * @file       ahrs.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      INSGPS Test Program
+ * @see        The GNU Public License (GPL) Version 3
+ * 
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "ahrs_timer.h"
+
+#define TIMER_RATE (8e6 / 128)
+void timer_start() 
+{
+	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();
+}
+
+uint32_t timer_count()
+{
+	return RTC_GetCounter(); 
+}
+
+uint32_t timer_rate() 
+{
+	return TIMER_RATE;
+}
\ No newline at end of file
diff --git a/flight/AHRS/inc/ahrs_timer.h b/flight/AHRS/inc/ahrs_timer.h
new file mode 100644
index 000000000..991fa4c61
--- /dev/null
+++ b/flight/AHRS/inc/ahrs_timer.h
@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS AHRS Control
+ * @brief The AHRS Modules perform
+ *
+ * @{ 
+ * @addtogroup AHRS_TIMER
+ * @brief Sets up a simple timer that can be polled to estimate idle time
+ * @{ 
+ *
+ *
+ * @file       ahrs.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      INSGPS Test Program
+ * @see        The GNU Public License (GPL) Version 3
+ * 
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef AHRS_TIMER
+#define AHRS_TIMER
+
+#include <pios.h>
+
+void timer_start();
+uint32_t timer_count();
+uint32_t timer_rate();
+
+#endif
\ No newline at end of file