diff --git a/Makefile b/Makefile
index 44d25fb522..db4024c8bc 100644
--- a/Makefile
+++ b/Makefile
@@ -81,7 +81,7 @@ ap autopilot: ac_h
 upload_fbw: hard_ac
 	cd $(AIRBORNE); $(MAKE) TARGET=fbw upload
 
-upload_ap: hard_ac
+upload_ap: ap
 	cd $(AIRBORNE); $(MAKE) TARGET=ap upload
 
 erase_fbw:
diff --git a/conf/Makefile.arm7 b/conf/Makefile.arm7
index a26d40fcb0..7c2b066ef3 100644
--- a/conf/Makefile.arm7
+++ b/conf/Makefile.arm7
@@ -83,7 +83,8 @@ OPT = s
 # Native formats for AVR-GCC's -g are stabs [default], or dwarf-2.
 # AVR (extended) COFF requires stabs, plus an avr-objcopy run.
 #DEBUG = stabs
-DEBUG = dwarf-2
+#DEBUG = dwarf-2
+DEBUG = 0
 
 # List any extra directories to look for include files here.
 #     Each directory must be seperated by a space.
@@ -117,8 +118,8 @@ ADEFS =  -D$(RUN_MODE)
 #
 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
 #CFLAGS = -g$(DEBUG)
-CFLAGS =  -g0
-CFLAGS += $(CDEFS) $(CINCS)
+#CFLAGS =  -g0
+CFLAGS = $(CDEFS) $(CINCS)
 CFLAGS += -O$(OPT)
 CFLAGS += -Wall -Wcast-qual -Wimplicit 
 # -Wcast-align # Incompatible with GPS message parsing
@@ -183,8 +184,10 @@ endif
 #LPC21ISP = lpc21isp
 LPC21ISP = lpc21isp
 LPC21ISP_PORT = /dev/ttyS0
-LPC21ISP_BAUD = 115200
-LPC21ISP_XTAL = 14746
+#LPC21ISP_BAUD = 115200
+#LPC21ISP_XTAL = 14746
+LPC21ISP_BAUD = 38400
+LPC21ISP_XTAL = 12000
 LPC21ISP_FLASHFILE = $(TARGET).hex
 # verbose output:
 #LPC21ISP_DEBUG = -debug
diff --git a/conf/autopilot/tiny_test.makefile b/conf/autopilot/tiny_test.makefile
index 40fbe4466d..de1c29ae4a 100644
--- a/conf/autopilot/tiny_test.makefile
+++ b/conf/autopilot/tiny_test.makefile
@@ -8,5 +8,6 @@ ap.TARGET = autopilot
 ap.TARGETDIR = autopilot
 ap.CFLAGS += -DAP
 ap.CFLAGS +=  -DGPS -DUBX
-ap.srcs = inter_mcu.c pid.c estimator.c gps_ubx.c gps.c nav.c cam.c main_ap.c mainloop.c main.c $(SRC_ARCH)/uart.c $(SRC_ARCH)/armVIC.c
+ap.srcs = inter_mcu.c pid.c estimator.c cam.c main_ap.c mainloop.c main.c $(SRC_ARCH)/uart.c $(SRC_ARCH)/armVIC.c
+ap.srcs += gps_ubx.c gps.c nav.c
 # ap.srcs += $(SRC_ARCH)/modem.c $(SRC_ARCH)/adc_ap.c $(SRC_ARCH)/uart_ap.c $(SRC_ARCH)/servo.c
diff --git a/sw/airborne/arm7/Makefile b/sw/airborne/arm7/Makefile
index 08c8430efe..788d5b19fa 100644
--- a/sw/airborne/arm7/Makefile
+++ b/sw/airborne/arm7/Makefile
@@ -97,7 +97,8 @@ OPT = s
 # Native formats for AVR-GCC's -g are stabs [default], or dwarf-2.
 # AVR (extended) COFF requires stabs, plus an avr-objcopy run.
 #DEBUG = stabs
-DEBUG = dwarf-2
+#DEBUG = dwarf-2
+DEBUG = 0
 
 # List any extra directories to look for include files here.
 #     Each directory must be seperated by a space.
@@ -131,8 +132,9 @@ ADEFS =  -D$(RUN_MODE)
 #
 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
 #CFLAGS = -g$(DEBUG)
-CFLAGS =  -g0
-CFLAGS += $(CDEFS) $(CINCS)
+#CFLAGS =  -g0
+
+CFLAGS = $(CDEFS) $(CINCS)
 CFLAGS += -O$(OPT)
 CFLAGS += -Wall -Wcast-align -Wcast-qual -Wimplicit 
 CFLAGS += -Wpointer-arith -Wswitch
diff --git a/sw/airborne/arm7/config.h b/sw/airborne/arm7/config.h
index cf8a439c38..48282b9557 100644
--- a/sw/airborne/arm7/config.h
+++ b/sw/airborne/arm7/config.h
@@ -20,12 +20,12 @@
 #include "LPC21xx.h"
 
 // some handy DEFINES
-#ifndef FALSE
-#define FALSE               0
-#ifndef TRUE
-#define TRUE                !FALSE
-#endif
-#endif
+//#ifndef FALSE
+//#define FALSE               0
+//#ifndef TRUE
+//#define TRUE                !FALSE
+//#endif
+//#endif
 
 #ifndef BIT
 #define BIT(n)              (1L << (n))
@@ -43,8 +43,9 @@ extern void abort(void);
 
 // PLL setup values are computed within the LPC include file
 // It relies upon the following defines
-#define FOSC                (14745600)  // Master Oscillator Freq.
-#define PLL_MUL             (4)         // PLL Multiplier
+//#define FOSC                (14745600)  // Master Oscillator Freq.
+#define FOSC                (12000000)  // Master Oscillator Freq.
+#define PLL_MUL             (5)         // PLL Multiplier
 #define CCLK                (FOSC * PLL_MUL) // CPU Clock Freq.
 
 // Pheripheral Bus Speed Divider
@@ -73,7 +74,7 @@ extern void abort(void);
 
 // Port Bit Definitions & Macros:    Description - initial conditions
 
-// The following defines are for the Olimex (LPC2138)
+// The following defines are for the Tiny v0.9)
 // PIO 0
 #define TXD0_BIT              BIT(0)      // used by UART0
 #define RXD0_BIT              BIT(1)      // used by UART0
@@ -90,8 +91,8 @@ extern void abort(void);
 //#define P0_09_UNUSED_BIT      BIT(9)      // P0.09 unused - low output
 #define P0_10_UNUSED_BIT      BIT(10)     // P0.10 unused - low output
 #define P0_11_UNUSED_BIT      BIT(11)     // P0.11 unused - low output
-#define LED1_BIT              BIT(12)     // P0.12 LED1 low active
-#define LED2_BIT              BIT(13)     // P0.13 LED1 low active
+#define P0_12_UNUSED_BIT      BIT(12)     // P0.12 unused - low output
+#define P0_13_UNUSED_BIT      BIT(13)     // P0.13 unused - low output
 #define P0_14_UNUSED_BIT      BIT(14)     // P0.14 unused - low output
 #define SW1_BIT               BIT(15)     // P0.15 Switch 1 - active low input
 #define SW2_BIT               BIT(16)     // P0.16 Switch 2 - active low input
@@ -131,7 +132,7 @@ extern void abort(void);
 #define P1_16_UNUSED_BIT      BIT(16)     // P1.16 unused - low output
 #define P1_17_UNUSED_BIT      BIT(17)     // P1.17 unused - low output
 #define P1_18_UNUSED_BIT      BIT(18)     // P1.18 unused - low output
-#define P1_19_UNUSED_BIT      BIT(19)     // P1.19 unused - low output
+#define LED_2_BIT             BIT(19)     // P1.19 unused - low output
 #define SERV0_DATA_BIT        BIT(20)     // P1.20 unused - low output
 #define SERV0_RESET_BIT       BIT(21)     // P1.21 unused - low output
 #define P1_22_UNUSED_BIT      BIT(22)     // P1.22 unused - low output
@@ -140,7 +141,7 @@ extern void abort(void);
 #define P1_25_UNUSED_BIT      BIT(25)     // P1.25 unused - low output
 #define P1_26_UNUSED_BIT      BIT(26)     // P1.26 unused - low output
 #define P1_27_UNUSED_BIT      BIT(27)     // P1.27 unused - low output
-#define P1_28_UNUSED_BIT      BIT(28)     // P1.28 unused - low output
+#define LED_1_BIT             BIT(28)     // P1.28 unused - low output
 #define SERV1_RESET_BIT       BIT(29)     // P1.29 unused - low output
 #define SERV1_DATA_BIT        BIT(30)     // P1.30 unused - low output
 #define P1_31_UNUSED_BIT      BIT(31)     // P1.31 unused - low output
@@ -160,8 +161,8 @@ extern void abort(void);
 					 SERV1_CLOCK_BIT  | \
 	                                 P0_10_UNUSED_BIT | \
 					 P0_11_UNUSED_BIT | \
-					 LED1_BIT         | \
-					 LED2_BIT         | \
+					 P0_12_UNUSED_BIT | \
+					 P0_13_UNUSED_BIT | \
 					 P0_14_UNUSED_BIT | \
 					 P0_17_UNUSED_BIT | \
 					 P0_18_UNUSED_BIT | \
@@ -212,7 +213,7 @@ extern void abort(void);
 					 P1_16_UNUSED_BIT | \
 					 P1_17_UNUSED_BIT | \
 					 P1_18_UNUSED_BIT | \
-					 P1_19_UNUSED_BIT | \
+					 LED_2_BIT        | \
 					 SERV0_DATA_BIT   | \
 					 SERV0_RESET_BIT  | \
 					 P1_22_UNUSED_BIT | \
@@ -221,7 +222,7 @@ extern void abort(void);
 					 P1_25_UNUSED_BIT | \
 					 P1_26_UNUSED_BIT | \
 					 P1_27_UNUSED_BIT | \
-					 P1_28_UNUSED_BIT | \
+					 LED_1_BIT        | \
 					 SERV1_RESET_BIT  | \
 					 SERV1_DATA_BIT   | \
 					 P1_31_UNUSED_BIT | \
diff --git a/sw/airborne/arm7/low_level_hw.h b/sw/airborne/arm7/low_level_hw.h
new file mode 100644
index 0000000000..df6c8642f7
--- /dev/null
+++ b/sw/airborne/arm7/low_level_hw.h
@@ -0,0 +1,98 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/*
+ *\brief ARM7 low level hardware initialisation 
+ * PLL, IOPORT, MAM, VIC
+ *
+ */
+
+#ifndef LOW_LEVEL_HW_H
+#define LOW_LEVEL_HW_H
+
+#include "LPC21xx.h"
+#include "config.h"
+
+static inline void low_level_init(void) {
+  /* set PLL multiplier & divisor. */
+  /* values computed from config.h */
+  PLLCFG = PLLCFG_MSEL | PLLCFG_PSEL;
+  /* enable PLL                    */
+  PLLCON = PLLCON_PLLE;
+  /* commit changes                */
+  PLLFEED = 0xAA;
+  PLLFEED = 0x55;
+
+  /* setup port pins */
+  IO0CLR = PIO0_ZERO_BITS;                // clear the ZEROs output
+  IO0SET = PIO0_ONE_BITS;                 // set the ONEs output
+  IO0DIR = PIO0_OUTPUT_BITS;              // set the output bit direction
+  
+  IO1CLR = PIO1_ZERO_BITS;                // clear the ZEROs output
+  IO1SET = PIO1_ONE_BITS;                 // set the ONEs output
+  IO1DIR = PIO1_OUTPUT_BITS;              // set the output bit direction
+  
+  /* wait for PLL lock */
+  while (!(PLLSTAT & PLLSTAT_LOCK))
+    continue;
+ 
+  /* enable & connect PLL */
+  PLLCON = PLLCON_PLLE | PLLCON_PLLC;
+  /* commit changes                */
+  PLLFEED = 0xAA;
+  PLLFEED = 0x55;
+
+  /* setup & enable the MAM */
+  MAMTIM = MAMTIM_CYCLES;
+  MAMCR = MAMCR_FULL;
+  
+  /* setup & enable the MAM */
+  MAMTIM = MAMTIM_CYCLES;
+  MAMCR = MAMCR_FULL;
+  
+  /* set the peripheral bus speed */
+  /* value computed from config.h */
+  VPBDIV = VPBDIV_VALUE;
+ 
+  /* set the interrupt controller defaults  */
+#if defined(RAM_RUN)
+  /* map interrupt vectors space into SRAM  */
+  MEMMAP = MEMMAP_SRAM;
+#elif defined(ROM_RUN)
+  /* map interrupt vectors space into FLASH */
+  MEMMAP = MEMMAP_FLASH;
+#else
+#error RUN_MODE not defined!
+#endif
+
+  /* clear all interrupts     */
+  VICIntEnClear = 0xFFFFFFFF;
+  /* clear all FIQ selections */
+  VICIntSelect = 0x00000000;
+  /* point unvectored IRQs to reset() */
+  VICDefVectAddr = (uint32_t)reset;
+
+}
+
+#endif /* LOW_LEVEL_HW_H */
diff --git a/sw/airborne/arm7/main.c b/sw/airborne/arm7/main.c
index 3a6457f5c1..f6cab56b94 100644
--- a/sw/airborne/arm7/main.c
+++ b/sw/airborne/arm7/main.c
@@ -114,6 +114,7 @@ static void periodic_task ( void ) {
       IO0CLR = LED1_BIT;
     else
       IO0SET = LED1_BIT; 
+    MODEM_PRINT_GPS();
     //    PRINT_ADC();
         DOWNLINK_SEND_IDENT(&AC_ID);
 
diff --git a/sw/airborne/arm7/sys_time_hw.h b/sw/airborne/arm7/sys_time_hw.h
new file mode 100644
index 0000000000..c3044c18be
--- /dev/null
+++ b/sw/airborne/arm7/sys_time_hw.h
@@ -0,0 +1,77 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/*
+ *\brief ARM7 timer functions 
+ *
+ */
+
+#ifndef SYS_TIME_HW_H
+#define SYS_TIME_HW_H
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "config.h" /* PCLK */
+
+static uint32_t last_periodic_event;
+
+/* T0 prescaler */
+#define T0_PCLK_DIV     3
+
+static inline void sys_time_init( void ) {
+  /* setup Timer 0 to count forever  */
+  /* reset & disable timer 0         */
+  T0TCR = TCR_RESET;
+  /* set the prescale divider        */
+  T0PR = T0_PCLK_DIV - 1;
+  /* disable match registers         */
+  T0MCR = 0;
+  /* disable compare registers       */
+  T0CCR = 0;
+  /* disable external match register */
+  T0EMR = 0;                          
+  /* enable timer 0                  */
+  T0TCR = TCR_ENABLE;
+  //  sysTICs = 0;
+}
+
+#define SysTicsOfSec(s)   (uint32_t)(s * PCLK / T0_PCLK_DIV + 0.5)
+#define FIFTY_MS          SysTicsOfSec( 50e-3 )
+
+#define PERIODIC_TASK_PERIOD FIFTY_MS
+
+static inline bool_t sys_time_periodic( void ) {
+  uint32_t now = T0TC;
+  if (now - last_periodic_event >= PERIODIC_TASK_PERIOD) {
+    last_periodic_event = now;
+    if (IO1PIN & LED_1_BIT)
+      IO1CLR = LED_1_BIT;
+    else
+      IO1SET = LED_1_BIT;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+#endif /* SYS_TIME_HW_H */
diff --git a/sw/airborne/arm7/ubx.h b/sw/airborne/arm7/ubx.h
deleted file mode 100644
index 44a25a4a06..0000000000
--- a/sw/airborne/arm7/ubx.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * Paparazzi autopilot $Id$
- *  
- * Copyright (C) 2004  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi 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 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
- *
- */
-
-/*
- * UBX protocol specific code
- *
-*/
-
-
-#ifndef UBX_H
-#define UBX_H
-
-#define GPS_FIX_VALID(gps_mode) (gps_mode == 3)
-
-#endif /* UBX_H */
diff --git a/sw/airborne/arm7/ubx_protocol.h b/sw/airborne/arm7/ubx_protocol.h
deleted file mode 100644
index 63bfdb2d4c..0000000000
--- a/sw/airborne/arm7/ubx_protocol.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/* Generated from conf/ubx.xml */
-/* Please DO NOT EDIT */
-
-#define UBX_SYNC1 0xB5
-#define UBX_SYNC2 0x62
-
-#define UBX_NAV_ID 0x01
-
-#define UBX_NAV_POSLLH_ID 0x02
-#define UBX_NAV_POSLLH_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
-#define UBX_NAV_POSLLH_LON(_ubx_payload) (*((int32_t*)(_ubx_payload+4)))
-#define UBX_NAV_POSLLH_LAT(_ubx_payload) (*((int32_t*)(_ubx_payload+8)))
-#define UBX_NAV_POSLLH_HEIGHT(_ubx_payload) (*((int32_t*)(_ubx_payload+12)))
-#define UBX_NAV_POSLLH_HMSL(_ubx_payload) (*((int32_t*)(_ubx_payload+16)))
-#define UBX_NAV_POSLLH_Hacc(_ubx_payload) (*((uint32_t*)(_ubx_payload+20)))
-#define UBX_NAV_POSLLH_Vacc(_ubx_payload) (*((uint32_t*)(_ubx_payload+24)))
-
-#define UBX_NAV_POSUTM_ID 0x08
-#define UBX_NAV_POSUTM_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
-#define UBX_NAV_POSUTM_EAST(_ubx_payload) (*((int32_t*)(_ubx_payload+4)))
-#define UBX_NAV_POSUTM_NORTH(_ubx_payload) (*((int32_t*)(_ubx_payload+8)))
-#define UBX_NAV_POSUTM_ALT(_ubx_payload) (*((int32_t*)(_ubx_payload+12)))
-#define UBX_NAV_POSUTM_ZONE(_ubx_payload) (*((int8_t*)(_ubx_payload+16)))
-#define UBX_NAV_POSUTM_HEM(_ubx_payload) (*((int8_t*)(_ubx_payload+17)))
-
-#define UBX_NAV_STATUS_ID 0x03
-#define UBX_NAV_STATUS_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
-#define UBX_NAV_STATUS_GPSfix(_ubx_payload) (*((uint8_t*)(_ubx_payload+4)))
-#define UBX_NAV_STATUS_Flags(_ubx_payload) (*((uint8_t*)(_ubx_payload+5)))
-#define UBX_NAV_STATUS_DiffS(_ubx_payload) (*((uint8_t*)(_ubx_payload+6)))
-#define UBX_NAV_STATUS_res(_ubx_payload) (*((uint8_t*)(_ubx_payload+7)))
-#define UBX_NAV_STATUS_TTFF(_ubx_payload) (*((uint32_t*)(_ubx_payload+8)))
-#define UBX_NAV_STATUS_MSSS(_ubx_payload) (*((uint32_t*)(_ubx_payload+12)))
-
-#define UBX_NAV_VELNED_ID 0x12
-#define UBX_NAV_VELNED_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
-#define UBX_NAV_VELNED_VEL_N(_ubx_payload) (*((int32_t*)(_ubx_payload+4)))
-#define UBX_NAV_VELNED_VEL_E(_ubx_payload) (*((int32_t*)(_ubx_payload+8)))
-#define UBX_NAV_VELNED_VEL_D(_ubx_payload) (*((int32_t*)(_ubx_payload+12)))
-#define UBX_NAV_VELNED_Speed(_ubx_payload) (*((uint32_t*)(_ubx_payload+16)))
-#define UBX_NAV_VELNED_GSpeed(_ubx_payload) (*((uint32_t*)(_ubx_payload+20)))
-#define UBX_NAV_VELNED_Heading(_ubx_payload) (*((int32_t*)(_ubx_payload+24)))
-#define UBX_NAV_VELNED_SAcc(_ubx_payload) (*((uint32_t*)(_ubx_payload+28)))
-#define UBX_NAV_VELNED_CAcc(_ubx_payload) (*((uint32_t*)(_ubx_payload+32)))
-
-#define UBX_NAV_SVINFO_ID 0x30
-#define UBX_NAV_SVINFO_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
-#define UBX_NAV_SVINFO_NCH(_ubx_payload) (*((uint8_t*)(_ubx_payload+4)))
-#define UBX_NAV_SVINFO_RES1(_ubx_payload) (*((uint8_t*)(_ubx_payload+5)))
-#define UBX_NAV_SVINFO_RES2(_ubx_payload) (*((uint16_t*)(_ubx_payload+6)))
-#define UBX_NAV_SVINFO_chn(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+8+12*_ubx_block)))
-#define UBX_NAV_SVINFO_SVID(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+9+12*_ubx_block)))
-#define UBX_NAV_SVINFO_Flags(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+10+12*_ubx_block)))
-#define UBX_NAV_SVINFO_QI(_ubx_payload,_ubx_block) (*((int8_t*)(_ubx_payload+11+12*_ubx_block)))
-#define UBX_NAV_SVINFO_CNO(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+12+12*_ubx_block)))
-#define UBX_NAV_SVINFO_Elev(_ubx_payload,_ubx_block) (*((int8_t*)(_ubx_payload+13+12*_ubx_block)))
-#define UBX_NAV_SVINFO_Azim(_ubx_payload,_ubx_block) (*((int16_t*)(_ubx_payload+14+12*_ubx_block)))
-#define UBX_NAV_SVINFO_PRRes(_ubx_payload,_ubx_block) (*((int32_t*)(_ubx_payload+16+12*_ubx_block)))
diff --git a/sw/airborne/avr/modem.c b/sw/airborne/avr/modem.c
index fe2f3e79c3..19077dc9f4 100644
--- a/sw/airborne/avr/modem.c
+++ b/sw/airborne/avr/modem.c
@@ -22,6 +22,10 @@
  *
  */
 
+/** \file modem_hw.c
+ *  \brief Handling of a CMX 469 on avr mega128 architecture
+ */
+
 #include <inttypes.h>
 #include <avr/io.h>
 #include <avr/signal.h>
@@ -42,6 +46,7 @@ uint8_t    tx_byte_idx;
 uint8_t ck_a, ck_b;
 
 void modem_init( void ) {
+  /* setup TIMER0 to generate a 4MHz clock */
   MODEM_OSC_DDR |= _BV(MODEM_OSC);
   OCR0 = 1; /* 4MhZ */
   TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
diff --git a/sw/airborne/avr/timer_ap.h b/sw/airborne/avr/sys_time_hw.h
similarity index 57%
rename from sw/airborne/avr/timer_ap.h
rename to sw/airborne/avr/sys_time_hw.h
index 197ed2ef44..b6eac33161 100644
--- a/sw/airborne/avr/timer_ap.h
+++ b/sw/airborne/avr/sys_time_hw.h
@@ -1,10 +1,7 @@
 /*
- * Paparazzi mcu0 timer functions
- *  
- * Copied from autopilot (autopilot.sf.net) thanx alot Trammell
+ * Paparazzi $Id$
  *
- * Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
- * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -21,71 +18,53 @@
  * You should have received a copy of the GNU General Public License
  * along with paparazzi; see the file COPYING.  If not, write to
  * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
+ * Boston, MA 02111-1307, USA.
  *
  */
 
-#ifndef TIMER_H
-#define TIMER_H
+/*
+ *\brief AVR timer functions 
+ *
+ */
+
+#ifndef SYS_TIME_HW_H
+#define SYS_TIME_HW_H
 
 #include "std.h"
-#include <avr/signal.h>
 #include <avr/io.h>
 
-
 /*
  * Enable Timer1 (16-bit) running at Clk/1 for the global system
- * clock.  This will be used for computing the servo pulse widths,
- * PPM decoding, etc.
+ * clock.
  *
  * Low frequency periodic tasks will be signaled by timer 0
  * running at Clk/1024.  For 16 Mhz clock, this will be every
  * 262144 microseconds, or 61 Hz.
  */
-static inline void timer_init( void ) {
+static inline void sys_time_init( void ) {
 
   /* Timer0: Modem clock is started in modem.h in ctc mode*/
 
   /* Timer1 @ Clk/1: System clock */
-  TCCR1A		= 0x00;
-  TCCR1B		= 0x01;
+  TCCR1A                = 0x00;
+  TCCR1B                = 0x01;
 
   /* Timer2 @ Clk/1024: Periodic clock */
-  TCCR2		= 0x05;
+  TCCR2         = 0x05;
 }
 
-
-/*
- * Retrieve the current time from the global clock in Timer1,
- * disabling interrupts to avoid stomping on the TEMP register.
- * If interrupts are already off, the non_atomic form can be used.
- */
-static inline uint16_t
-timer_now( void )
-{
-  return TCNT1;
-}
-
-static inline uint16_t
-timer_now_non_atomic( void )
-{
-  return TCNT1L;
-}
-
-
 /*
  *  Periodic tasks occur when Timer2 overflows.  Check and unset
- * the overflow bit.  We cycle through four possible periodic states,
- * so each state occurs every 30 Hz.
+ * the overflow bit.
+ *
  */
-static inline bool_t
-timer_periodic( void )
-{
+static inline bool_t sys_time_periodic( void ) {
   if( !bit_is_set( TIFR, TOV2 ) )
     return FALSE;
-
   TIFR = 1 << TOV2;
   return TRUE;
 }
 
-#endif
+
+
+#endif /* SYS_TIME_HW_H */
diff --git a/sw/airborne/gps.c b/sw/airborne/gps.c
index b33828622c..cb0c81361b 100644
--- a/sw/airborne/gps.c
+++ b/sw/airborne/gps.c
@@ -61,7 +61,6 @@ void estimator_update_state_gps( void ) {
  */
 void use_gps_pos( void ) {
   DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone);
-
   if (GPS_FIX_VALID(gps_mode)) {
     last_gps_msg_t = cputime;
     estimator_update_state_gps();
diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c
index 6e407e4365..4aa3b3d8b9 100644
--- a/sw/airborne/gps_ubx.c
+++ b/sw/airborne/gps_ubx.c
@@ -103,7 +103,7 @@ void gps_init( void ) {
 #define NAV_DYN_AIRBORNE_4G 7
 
 void gps_configure ( void ) {
-  static uint8_t gps_config_status = 0;
+  static uint8_t gps_config_status = 1;
   switch (gps_config_status) {
   case 0:
     UbxSend_CFG_PRT(0x01, 0x00, 0x0000, 0x000080C0, 0x00009600, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0000, 0x0000);
@@ -130,8 +130,6 @@ void gps_configure ( void ) {
   case 7:
     UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
     break;
-    
-
   }
   gps_config_status++;
 }
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 9c1db1f7cf..45378633d4 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -341,6 +341,7 @@ static void navigation_task( void ) {
   energy += (float)desired_gaz * (MILLIAMP_PER_PERCENT / MAX_PPRZ * 0.25);
 }
 
+
 #define PERIOD (256. * 1024. / CLOCK / 1000000.)
 
 /** Maximum time allowed for low battery level */
diff --git a/sw/airborne/mainloop.c b/sw/airborne/mainloop.c
index 445a0fbe01..6259382620 100644
--- a/sw/airborne/mainloop.c
+++ b/sw/airborne/mainloop.c
@@ -26,7 +26,9 @@
  
 #include "main_ap.h"
 #include "int.h"
-#include "timer_ap.h"
+//#include "timer_ap.h"
+#include "low_level_hw.h"
+#include "sys_time_hw.h"
 #include "adc_ap.h"
 #include "autopilot.h"
 #include "gps.h"
@@ -56,7 +58,8 @@
 
 void init_ap( void ) {
 #ifndef FBW /** Dual mcus : init done in main_fbw */
-  timer_init(); 
+  low_level_init();
+  sys_time_init(); 
 #ifdef ADC
   adc_init();
 #endif
@@ -102,7 +105,7 @@ void init_ap( void ) {
   /** - wait 0.5s (for modem init ?) */
   uint8_t init_cpt = 30;
   while (init_cpt) {
-    if (timer_periodic())
+    if (sys_time_periodic())
       init_cpt--;
   }
 #ifdef WAVECARD
diff --git a/sw/airborne/uart_ap.h b/sw/airborne/uart_ap.h
index 09a200a235..b12a872f17 100644
--- a/sw/airborne/uart_ap.h
+++ b/sw/airborne/uart_ap.h
@@ -32,9 +32,6 @@ extern void uart0_init_tx(void);
 extern void uart0_init_rx(void);
 //extern void uart1_init(void);
 
-extern void uart0_print_string(const uint8_t*);
-extern void uart0_print_hex(const uint8_t);
-extern void uart0_print_hex16(const uint16_t);
 extern void uart0_transmit(const uint8_t);
 extern void uart1_transmit(const uint8_t);