diff --git a/sw/airborne/avr/sys_time_hw.c b/sw/airborne/avr/sys_time_hw.c
index 79ee7e6815..30db5fae5f 100644
--- a/sw/airborne/avr/sys_time_hw.c
+++ b/sw/airborne/avr/sys_time_hw.c
@@ -7,6 +7,8 @@
 #include CONFIG
 #include "std.h"
 
+uint16_t cpu_time_ticks;
+
 #if CLOCK == 8
 volatile uint8_t tmr2_ov_cnt;
 volatile bool_t tmr2_overflow;
diff --git a/sw/airborne/avr/sys_time_hw.h b/sw/airborne/avr/sys_time_hw.h
index d0a491690f..5a26de7d7d 100644
--- a/sw/airborne/avr/sys_time_hw.h
+++ b/sw/airborne/avr/sys_time_hw.h
@@ -33,6 +33,8 @@
 #include "std.h"
 #include <avr/io.h>
 
+extern uint16_t cpu_time_ticks;
+
 #define F_CPU (CLOCK*1000000UL)
 
 /*
@@ -73,7 +75,8 @@ static inline void sys_time_init( void ) {
   TCCR3B = _BV(CS10);
 #endif
 
-  cpu_time = 0;
+  cpu_time_sec = 0;
+  cpu_time_ticks = 0;
 }
 
 
@@ -98,6 +101,7 @@ extern volatile uint8_t tmr2_ov_cnt;
 extern volatile bool_t tmr2_overflow;
 #endif
 
+#define TICKS_PER_SEC (CLOCK * 1e6 / 1024 * 256)
 
 #if CLOCK == 8
 static inline bool_t sys_time_periodic( void ) {
@@ -108,11 +112,16 @@ static inline bool_t sys_time_periodic( void ) {
   return (tmr2_ov_cnt & 0x1);
 }
 #else 
+//#define TMR2_PER_SEC 7812
 static inline bool_t sys_time_periodic( void ) {
   if( !bit_is_set( TIFR, TOV2 ) )
     return FALSE;
   TIFR = _BV(TOV2);
-
+  cpu_time_ticks += 256;
+  if (cpu_time_ticks > TICKS_PER_SEC) {
+    cpu_time_ticks -= TICKS_PER_SEC;
+    cpu_time_sec++;
+  }
   return TRUE;
 }
 #endif
diff --git a/sw/airborne/gps.c b/sw/airborne/gps.c
index 1ada7ab14a..7271ac7896 100644
--- a/sw/airborne/gps.c
+++ b/sw/airborne/gps.c
@@ -63,7 +63,7 @@ void estimator_update_state_gps( void ) {
  */
 void use_gps_pos( void ) {
   if (GpsFixValid()) {
-    last_gps_msg_t = cpu_time;
+    last_gps_msg_t = cpu_time_sec;
     estimator_update_state_gps();
   }
   DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone, &gps_nb_ovrn);