diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c
index 716b79061d..a7f587fe99 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.c
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -22,7 +22,7 @@
 /**
  * @file peripherals/mpu60x0_i2c.c
  *
- * Driver for the MPU-60X0 using SPI.
+ * Driver for the MPU-60X0 using I2C.
  *
  */
 
@@ -51,10 +51,7 @@ static void mpu60x0_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
   struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu);
   mpu_i2c->i2c_trans.buf[0] = _reg;
   mpu_i2c->i2c_trans.buf[1] = _val;
-  mpu_i2c->i2c_trans.len_r = 0;
-  mpu_i2c->i2c_trans.len_w = 2;
-  mpu_i2c->i2c_trans.type = I2CTransTx;
-  i2c_submit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans));
+  i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu->i2c_trans.slave_addr, 2);
 }
 
 // Configuration function called once before normal use
@@ -71,12 +68,9 @@ void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
 void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
 {
   if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
-    mpu->i2c_trans.len_w = 1;
-    mpu->i2c_trans.len_r = 15; // FIXME external data
-    mpu->i2c_trans.type = I2CTransTxRx;
     /* set read bit and multiple byte bit, then address */
     mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS;
-    i2c_submit(mpu->i2c_p, &(mpu->i2c_trans));
+    i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, 15);
   }
 }
 
diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h
index 1b745e5f9d..e1d460490c 100644
--- a/sw/airborne/peripherals/mpu60x0_regs.h
+++ b/sw/airborne/peripherals/mpu60x0_regs.h
@@ -43,8 +43,8 @@
 // FIFO
 #define MPU60X0_REG_FIFO_EN         0X23
 #define MPU60X0_REG_FIFO_COUNT_H    0x72
-#define MPU60X0_REG_FIFO_COUNT_L	  0x73
-#define MPU60X0_REG_FIFO_R_W		    0x74
+#define MPU60X0_REG_FIFO_COUNT_L	0x73
+#define MPU60X0_REG_FIFO_R_W		0x74
 
 // Measurement Settings
 #define MPU60X0_REG_SMPLRT_DIV      0X19
@@ -57,31 +57,31 @@
 #define MPU60X0_REG_I2C_MST_STATUS  0X36
 #define MPU60X0_REG_I2C_MST_DELAY   0X67
 // Slave 0
-#define MPU60X0_REG_I2C_SLV0_ADDR	  0X25	// i2c addr
-#define MPU60X0_REG_I2C_SLV0_REG	  0X26	// slave reg
-#define MPU60X0_REG_I2C_SLV0_CTRL	  0X27	// set-bits
-#define MPU60X0_REG_I2C_SLV0_DO		  0X63	// DO
+#define MPU60X0_REG_I2C_SLV0_ADDR	0X25	// i2c addr
+#define MPU60X0_REG_I2C_SLV0_REG	0X26	// slave reg
+#define MPU60X0_REG_I2C_SLV0_CTRL	0X27	// set-bits
+#define MPU60X0_REG_I2C_SLV0_DO		0X63	// DO
 // Slave 1
-#define MPU60X0_REG_I2C_SLV1_ADDR	  0X28	// i2c addr
-#define MPU60X0_REG_I2C_SLV1_REG	  0X29	// slave reg
-#define MPU60X0_REG_I2C_SLV1_CTRL	  0X2A	// set-bits
-#define MPU60X0_REG_I2C_SLV1_DO		  0X64	// DO
+#define MPU60X0_REG_I2C_SLV1_ADDR	0X28	// i2c addr
+#define MPU60X0_REG_I2C_SLV1_REG	0X29	// slave reg
+#define MPU60X0_REG_I2C_SLV1_CTRL	0X2A	// set-bits
+#define MPU60X0_REG_I2C_SLV1_DO		0X64	// DO
 // Slave 2
-#define MPU60X0_REG_I2C_SLV2_ADDR	  0X2B	// i2c addr
-#define MPU60X0_REG_I2C_SLV2_REG	  0X2C	// slave reg
-#define MPU60X0_REG_I2C_SLV2_CTRL	  0X2D	// set-bits
-#define MPU60X0_REG_I2C_SLV2_DO		  0X65	// DO
+#define MPU60X0_REG_I2C_SLV2_ADDR	0X2B	// i2c addr
+#define MPU60X0_REG_I2C_SLV2_REG	0X2C	// slave reg
+#define MPU60X0_REG_I2C_SLV2_CTRL	0X2D	// set-bits
+#define MPU60X0_REG_I2C_SLV2_DO		0X65	// DO
 // Slave 3
-#define MPU60X0_REG_I2C_SLV3_ADDR	  0X2E	// i2c addr
-#define MPU60X0_REG_I2C_SLV3_REG	  0X2F	// slave reg
-#define MPU60X0_REG_I2C_SLV3_CTRL	  0X30	// set-bits
-#define MPU60X0_REG_I2C_SLV3_DO		  0X66	// DO
+#define MPU60X0_REG_I2C_SLV3_ADDR	0X2E	// i2c addr
+#define MPU60X0_REG_I2C_SLV3_REG	0X2F	// slave reg
+#define MPU60X0_REG_I2C_SLV3_CTRL	0X30	// set-bits
+#define MPU60X0_REG_I2C_SLV3_DO		0X66	// DO
 // Slave 4 - special
-#define MPU60X0_REG_I2C_SLV4_ADDR	  0X31	// i2c addr
-#define MPU60X0_REG_I2C_SLV4_REG	  0X32	// slave reg
-#define MPU60X0_REG_I2C_SLV4_DO		  0X33	// DO
-#define MPU60X0_REG_I2C_SLV4_CTRL	  0X34	// set-bits
-#define MPU60X0_REG_I2C_SLV4_DI		  0X35	// DI
+#define MPU60X0_REG_I2C_SLV4_ADDR	0X31	// i2c addr
+#define MPU60X0_REG_I2C_SLV4_REG	0X32	// slave reg
+#define MPU60X0_REG_I2C_SLV4_DO		0X33	// DO
+#define MPU60X0_REG_I2C_SLV4_CTRL	0X34	// set-bits
+#define MPU60X0_REG_I2C_SLV4_DI		0X35	// DI
 
 // Interrupt
 #define MPU60X0_REG_INT_PIN_CFG     0X37
@@ -117,7 +117,7 @@
 // MPU60X0 Definitions
 
 #define MPU60X0_REG_WHO_AM_I        0X75
-#define MPU60X0_WHOAMI_REPLY		    0x68
+#define MPU60X0_WHOAMI_REPLY		0x68
 
 /** Digital Low Pass Filter Options
  *  DLFP is affecting both gyro and accels,