diff --git a/conf/autopilot/subsystems/shared/imu_navgo.makefile b/conf/autopilot/subsystems/shared/imu_navgo.makefile
index 93d770ddf4..e10385bbc3 100644
--- a/conf/autopilot/subsystems/shared/imu_navgo.makefile
+++ b/conf/autopilot/subsystems/shared/imu_navgo.makefile
@@ -5,12 +5,21 @@ IMU_NAVGO_CFLAGS += -DIMU_TYPE_H=\"boards/navgo/imu_navgo.h\"
 IMU_NAVGO_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
                     $(SRC_BOARD)/imu_navgo.c
 
-IMU_NAVGO_CFLAGS += -DHMC58XX_I2C_DEVICE=i2c1
-IMU_NAVGO_SRCS += peripherals/hmc58xx.c
-
+IMU_NAVGO_I2C_DEVICE=i2c1
 IMU_NAVGO_CFLAGS += -DUSE_I2C
-IMU_NAVGO_CFLAGS += -DUSE_I2C1
-IMU_NAVGO_CFLAGS += -DIMU_NAVGO_I2C_DEVICE=i2c1
+IMU_NAVGO_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=25 -DI2C1_SCLH=25
+
+IMU_NAVGO_CFLAGS += -DITG3200_I2C_DEVICE=$(IMU_NAVGO_I2C_DEVICE)
+IMU_NAVGO_CFLAGS += -DITG3200_I2C_ADDR=ITG3200_ADDR_ALT
+IMU_NAVGO_CFLAGS += -DITG3200_SMPLRT_DIV=1
+IMU_NAVGO_SRCS += peripherals/itg3200.c
+
+IMU_NAVGO_CFLAGS += -DADXL345_I2C_DEVICE=$(IMU_NAVGO_I2C_DEVICE)
+IMU_NAVGO_CFLAGS += -DADXL345_I2C_ADDR=ADXL345_ADDR_ALT
+IMU_NAVGO_SRCS += peripherals/adxl345.i2c.c
+
+IMU_NAVGO_CFLAGS += -DHMC58XX_I2C_DEVICE=$(IMU_NAVGO_I2C_DEVICE)
+IMU_NAVGO_SRCS += peripherals/hmc58xx.c
 
 ap.CFLAGS += $(IMU_NAVGO_CFLAGS)
 ap.srcs   += $(IMU_NAVGO_SRCS)
diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c
index 44c721fcf3..f4a8e4d75b 100644
--- a/sw/airborne/boards/navgo/imu_navgo.c
+++ b/sw/airborne/boards/navgo/imu_navgo.c
@@ -35,10 +35,20 @@
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 
-
 // Peripherials
-#include "peripherals/itg3200.h"
-#include "peripherals/adxl345.h"
+
+// Configure ITG3200
+// ITG3200_I2C_DEVICE IMU_UMARIM_I2C_DEVICE
+// ITG3200_I2C_ADDR ITG3200_ADDR_ALT
+// ITG3200_SMPLRT_DIV 1
+#include "peripherals/itg3200.extra.h"
+
+// Configure ADXL345
+// ADXL345_I2C_DEVICE IMU_UMARIM_I2C_DEVICE
+// ADXL345_I2C_ADDR ADXL345_ADDR_ALT
+#include "peripherals/adxl345.extra_i2c.h"
+
+// Configure HMC58XX
 #include "peripherals/hmc58xx.h"
 
 // Results
@@ -46,75 +56,17 @@ volatile bool_t gyr_valid;
 volatile bool_t acc_valid;
 volatile bool_t mag_valid;
 
-// Communication
-struct i2c_transaction imu_navgo_itg3200;
-struct i2c_transaction imu_navgo_adxl345;
-
-#ifndef PERIODIC_FREQUENCY
-#define PERIODIC_FREQUENCY 60
-#endif
-
 void imu_impl_init(void)
 {
   /////////////////////////////////////////////////////////////////////
   // ITG3200
-  imu_navgo_itg3200.type = I2CTransTx;
-  imu_navgo_itg3200.slave_addr = ITG3200_ADDR_ALT;
-  imu_navgo_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
-  /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
-  imu_navgo_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
-  imu_navgo_itg3200.len_w = 2;
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_itg3200);
-  while(imu_navgo_itg3200.status == I2CTransPending);
-
-  /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
-  imu_navgo_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
-  imu_navgo_itg3200.buf[1] = 1;  // 500Hz
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_itg3200);
-  while(imu_navgo_itg3200.status == I2CTransPending);
-
-  /* switch to gyroX clock */
-  imu_navgo_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
-  imu_navgo_itg3200.buf[1] = 0x01;
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_itg3200);
-  while(imu_navgo_itg3200.status == I2CTransPending);
-
-  /* no interrupts for now, but set data ready interrupt to enable reading status bits */
-  imu_navgo_itg3200.buf[0] = ITG3200_REG_INT_CFG;
-  imu_navgo_itg3200.buf[1] = 0x00;
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_itg3200);
-  while(imu_navgo_itg3200.status == I2CTransPending);
+  itg3200_init();
 
   /////////////////////////////////////////////////////////////////////
   // ADXL345
+  adxl345_init();
 
-  /* set data rate to 100Hz */
-  imu_navgo_adxl345.slave_addr = ADXL345_ADDR_ALT;
-  imu_navgo_adxl345.type = I2CTransTx;
-  imu_navgo_adxl345.buf[0] = ADXL345_REG_BW_RATE;
-  imu_navgo_adxl345.buf[1] = 0x0a;  // normal power and 100Hz sampling, 50Hz BW
-  imu_navgo_adxl345.len_w = 2;
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_adxl345);
-  while(imu_navgo_adxl345.status == I2CTransPending);
-
-  /* switch to measurement mode */
-  imu_navgo_adxl345.type = I2CTransTx;
-  imu_navgo_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
-  imu_navgo_adxl345.buf[1] = 1<<3;
-  imu_navgo_adxl345.len_w = 2;
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_adxl345);
-  while(imu_navgo_adxl345.status == I2CTransPending);
-
-  /* Set range to 16g but keeping full resolution of 3.9 mV/g */
-  imu_navgo_adxl345.type = I2CTransTx;
-  imu_navgo_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
-  imu_navgo_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03;  // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
-  imu_navgo_adxl345.len_w = 2;
-  i2c_submit(&IMU_NAVGO_I2C_DEVICE,&imu_navgo_adxl345);
-  while(imu_navgo_adxl345.status == I2CTransPending);
-
-
-  ///////////////////////////////////////////////////////////////////////
+  /////////////////////////////////////////////////////////////////////
   // HMC58XX
   hmc58xx_init();
 }
@@ -122,26 +74,13 @@ void imu_impl_init(void)
 void imu_periodic( void )
 {
   // Start reading the latest gyroscope data
-  if (imu_navgo_itg3200.status == I2CTransDone) {
-    imu_navgo_itg3200.type = I2CTransTxRx;
-    imu_navgo_itg3200.len_r = 9;
-    imu_navgo_itg3200.len_w = 1;
-    imu_navgo_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
-    i2c_submit(&IMU_NAVGO_I2C_DEVICE, &imu_navgo_itg3200);
-  }
+  itg3200_periodic();
 
   // Start reading the latest accelerometer data
-  if (imu_navgo_adxl345.status == I2CTransDone) {
-    imu_navgo_adxl345.type = I2CTransTxRx;
-    imu_navgo_adxl345.len_r = 6;
-    imu_navgo_adxl345.len_w = 1;
-    imu_navgo_adxl345.buf[0] = ADXL345_REG_DATA_X0;
-    i2c_submit(&IMU_NAVGO_I2C_DEVICE, &imu_navgo_adxl345);
-  }
+  adxl345_periodic();
 
   // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
-  //RunOnceEvery(10,hmc58xx_periodic());
-  hmc58xx_periodic();
+  RunOnceEvery(10,hmc58xx_periodic());
 
   RunOnceEvery(20,imu_navgo_downlink_raw());
 }
@@ -157,51 +96,21 @@ void imu_navgo_downlink_raw( void )
 
 void imu_navgo_event( void )
 {
-  int32_t x, y, z;
 
   // If the itg3200 I2C transaction has succeeded: convert the data
-  if (imu_navgo_itg3200.status == I2CTransSuccess) {
-#define ITG_STA_DAT_OFFSET 3
-    x = (int16_t) ((imu_navgo_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | imu_navgo_itg3200.buf[1+ITG_STA_DAT_OFFSET]);
-    y = (int16_t) ((imu_navgo_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | imu_navgo_itg3200.buf[3+ITG_STA_DAT_OFFSET]);
-    z = (int16_t) ((imu_navgo_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | imu_navgo_itg3200.buf[5+ITG_STA_DAT_OFFSET]);
-
-    // Is this is new data
-    if (imu_navgo_itg3200.buf[0] & 0x01) {
-      gyr_valid = TRUE;
-    }
-
-    RATES_ASSIGN(imu.gyro_unscaled,
-        (/*IMU_GYRO_P_SIGN * */x),
-        (/*IMU_GYRO_Q_SIGN * */y),
-        (/*IMU_GYRO_R_SIGN * */z));
-
-    imu_navgo_itg3200.status = I2CTransDone;
+  itg3200_event();
+  if (itg3200_data_available) {
+    RATES_COPY(imu.gyro_unscaled, itg3200_data);
+    itg3200_data_available = FALSE;
+    gyr_valid = TRUE;
   }
-  // Reset status if transaction fails
-  if (imu_navgo_itg3200.status == I2CTransFailed) {
-    imu_navgo_itg3200.status = I2CTransDone;
-  }
-
 
   // If the adxl345 I2C transaction has succeeded: convert the data
-  if (imu_navgo_adxl345.status == I2CTransSuccess)
-  {
-    x = (int16_t) ((imu_navgo_adxl345.buf[1] << 8) | imu_navgo_adxl345.buf[0]);
-    y = (int16_t) ((imu_navgo_adxl345.buf[3] << 8) | imu_navgo_adxl345.buf[2]);
-    z = (int16_t) ((imu_navgo_adxl345.buf[5] << 8) | imu_navgo_adxl345.buf[4]);
-
-    VECT3_ASSIGN(imu.accel_unscaled,
-        (/*IMU_ACCEL_X_SIGN * */y),
-        (/*IMU_ACCEL_Y_SIGN * */-x),
-        (/*IMU_ACCEL_Z_SIGN * */z));
-
+  adxl345_event();
+  if (adxl345_data_available) {
+    VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
+    adxl345_data_available = FALSE;
     acc_valid = TRUE;
-    imu_navgo_adxl345.status = I2CTransDone;
-  }
-  // Reset status if transaction fails
-  if (imu_navgo_adxl345.status == I2CTransFailed) {
-    imu_navgo_adxl345.status = I2CTransDone;
   }
 
   // HMC58XX event task