diff --git a/sw/airborne/arm7/uart_hw.c b/sw/airborne/arm7/uart_hw.c
index 47904aefc4..8ffa59acc0 100644
--- a/sw/airborne/arm7/uart_hw.c
+++ b/sw/airborne/arm7/uart_hw.c
@@ -185,8 +185,6 @@ void uart1_ISR(void) __attribute__((naked));
 uint8_t  uart1_rx_buffer[UART1_RX_BUFFER_SIZE];
 uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx;
 
-static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
-
 uint8_t  uart1_tx_buffer[UART1_TX_BUFFER_SIZE];
 uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx;
 uint8_t  uart1_tx_running;
@@ -206,7 +204,7 @@ bool_t uart1_check_free_space( uint8_t len) {
 
 void uart1_init_rx( void ) {}
 
-static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
+void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
   // set port pins for UART1
   PINSEL0 = (PINSEL0 & ~U1_PINMASK) | U1_PINSEL;
 
diff --git a/sw/airborne/arm7/uart_hw.h b/sw/airborne/arm7/uart_hw.h
index be39717a69..2ab93e7a75 100644
--- a/sw/airborne/arm7/uart_hw.h
+++ b/sw/airborne/arm7/uart_hw.h
@@ -53,6 +53,7 @@ extern uint8_t uart0_rx_buffer[UART0_RX_BUFFER_SIZE];
 
 extern uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx;
 extern uint8_t uart1_rx_buffer[UART1_RX_BUFFER_SIZE];
+extern void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
 
 #define Uart1ChAvailable() (uart1_rx_insert_idx != uart1_rx_extract_idx)
 
@@ -62,6 +63,6 @@ extern uint8_t uart1_rx_buffer[UART1_RX_BUFFER_SIZE];
    ret;                                                 \
 })
 
-
+extern uint8_t uart1_tx_running;
 
 #endif /* UART_HW_H */
diff --git a/sw/airborne/gps.h b/sw/airborne/gps.h
index cb5860d808..e42f128a94 100644
--- a/sw/airborne/gps.h
+++ b/sw/airborne/gps.h
@@ -55,15 +55,21 @@ extern int32_t gps_lat, gps_lon; /* 1e7 deg */
 extern uint16_t gps_PDOP;
 extern uint32_t gps_Pacc, gps_Sacc;
 extern uint8_t gps_numSV;
+extern uint8_t gps_status_config;
 
 
 extern uint16_t last_gps_msg_t; /** cputime of the last gps message */
 
+#define GPS_CONFIG_INIT 0
+#define GPS_CONFIG_DONE 7
+
 void gps_init( void );
 void gps_configure( void );
 void parse_gps_msg( void );
 void estimator_update_state_gps( void );
 void use_gps_pos( void );
+void gps_configure_uart( void );
+
 
 extern volatile uint8_t gps_msg_received;
 extern bool_t gps_pos_available;
@@ -94,6 +100,10 @@ extern struct svinfo gps_svinfos[GPS_NB_CHANNELS];
 #define GpsBuffer() GpsLink(ChAvailable())
 #define ReadGpsBuffer() { while (GpsLink(ChAvailable())&&!gps_msg_received) parse_ubx(GpsLink(Getch())); }
 #define GpsUartSend1(c) GpsLink(Transmit(c))
+#define GpsUartInitParam(_a,_b,_c) GpsLink(InitParam(_a,_b,_c))
+#define GpsUartRunning GpsLink(TxRunning)
+
+
 #endif /** !SITL */
 
 
diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c
index d046426dca..64d5808b8a 100644
--- a/sw/airborne/gps_ubx.c
+++ b/sw/airborne/gps_ubx.c
@@ -102,8 +102,13 @@ static uint8_t  ubx_msg_idx;
 static uint8_t ck_a, ck_b;
 uint8_t send_ck_a, send_ck_b;
 
+uint8_t gps_status_config;
+
 void gps_init( void ) {
   ubx_status = UNINIT;
+#ifdef GPS_CONFIGURE
+  gps_status_config = GPS_CONFIG_INIT;
+#endif
 }
 
 
@@ -115,17 +120,53 @@ void gps_init( void ) {
 #define NAV_DYN_AIRBORNE_2G 6
 #define NAV_DYN_AIRBORNE_4G 7
 
-void gps_configure ( void ) {
-  UbxSend_CFG_PRT(0x01, 0x00, 0x0000, 0x000080C0, 19200, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0000, 0x0000);
-  UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
-  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
-  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
-  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0);
-  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0);
-  UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
-  UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
+#ifdef GPS_CONFIGURE
+/* GPS dynamic configuration */
+
+#include "uart.h"
+
+/* Configure the GPS baud rate using the current uart baud rate. Busy
+   wait for the end of the transmit. Then, BEFORE waiting for the ACK,
+   change the uart rate. */
+void gps_configure_uart ( void ) {
+  UbxSend_CFG_PRT(0x01, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);  
+  while (GpsUartRunning) ; /* FIXME */
+  GpsUartInitParam( UART_BAUD(GPS_BAUD),  UART_8N1, UART_FIFO_8);
 }
 
+/* GPS configuration. Must be called on ack message reception while 
+   gps_status_config < GPS_CONFIG_DONE */
+void gps_configure ( void ) {
+  if (ubx_class == UBX_ACK_ID) {
+    if (ubx_id == UBX_ACK_ACK_ID) {
+      gps_status_config++;
+    }
+  }
+  switch (gps_status_config) {
+  case 0:
+    UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
+    break;
+  case 1:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
+    break;
+  case 2:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
+    break;
+  case 3:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0);
+    break;
+  case 4:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0);
+    break;
+  case 5:
+    UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
+    break;
+  case 6:
+    UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);   
+    break;
+  }
+}
+#endif /* GPS_CONFIGURE */
 
 void ubxsend_cfg_rst(uint16_t bbr , uint8_t val) {
   UbxSend_CFG_RST(bbr, val, 0x00);
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 9dcdb73543..3b38a9cab2 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -655,9 +655,6 @@ void init_ap( void ) {
   /** Reset the wavecard during the init pause */
   wc_reset();
 #endif
-#if defined GPS && defined GPS_CONFIGURE
-  gps_configure();
-#endif
 
   /************ Internal status ***************/
   h_ctl_init();
@@ -679,6 +676,10 @@ void init_ap( void ) {
       init_cpt--;
   }
 
+#if defined GPS_CONFIGURE
+  gps_configure_uart();
+#endif
+
 #if defined DATALINK
 
 #if DATALINK == XBEE
@@ -715,7 +716,12 @@ void event_task_ap( void ) {
 #endif
   if (gps_msg_received) {
     /* parse and use GPS messages */
-    parse_gps_msg();
+#ifdef GPS_CONFIGURE
+    if (gps_status_config < GPS_CONFIG_DONE)		
+      gps_configure();
+    else
+#endif
+      parse_gps_msg();
     gps_msg_received = FALSE;
     if (gps_pos_available) {
       use_gps_pos();
diff --git a/sw/airborne/uart.h b/sw/airborne/uart.h
index e9a427cbaa..47e1110dea 100644
--- a/sw/airborne/uart.h
+++ b/sw/airborne/uart.h
@@ -63,4 +63,7 @@ bool_t uart1_check_free_space( uint8_t len);
 #define Uart0SendMessage() {}
 #define Uart1SendMessage() {}
 
+#define Uart1TxRunning uart1_tx_running
+#define Uart1InitParam uart1_init_param
+
 #endif /* UART_H */