diff --git a/conf/firmwares/subsystems/shared/imu_apogee.makefile b/conf/firmwares/subsystems/shared/imu_apogee.makefile
index 36d120740c..c3e9e4da9b 100644
--- a/conf/firmwares/subsystems/shared/imu_apogee.makefile
+++ b/conf/firmwares/subsystems/shared/imu_apogee.makefile
@@ -13,6 +13,7 @@ IMU_APOGEE_I2C_DEV=i2c1
 IMU_APOGEE_CFLAGS += -DUSE_I2C -DUSE_I2C1
 
 IMU_APOGEE_CFLAGS += -DIMU_APOGEE_I2C_DEV=$(IMU_APOGEE_I2C_DEV)
+IMU_APOGEE_SRCS += peripherals/mpu60x0.c
 IMU_APOGEE_SRCS += peripherals/mpu60x0_i2c.c
 
 # with default APOGEE_SMPLRT_DIV (gyro output 100Hz)
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
index b1ab84d12e..3ad6e9a953 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
@@ -45,6 +45,7 @@ endif
 IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2.h\"
 IMU_ASPIRIN_2_SRCS    = $(SRC_SUBSYSTEMS)/imu.c
 IMU_ASPIRIN_2_SRCS   += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2.c
+IMU_ASPIRIN_2_SRCS   += peripherals/mpu60x0.c
 IMU_ASPIRIN_2_SRCS   += peripherals/mpu60x0_spi.c
 
 # Magnetometer
diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c
new file mode 100644
index 0000000000..34598ac5c9
--- /dev/null
+++ b/sw/airborne/peripherals/mpu60x0.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * 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.
+ */
+
+/**
+ * @file peripherals/mpu60x0.c
+ *
+ * MPU-60X0 driver common functions (I2C and SPI).
+ *
+ * Still needs the either the I2C or SPI specific implementation.
+ */
+
+#include "peripherals/mpu60x0.h"
+
+void mpu60x0_set_default_config(struct Mpu60x0Config *c)
+{
+  c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
+  c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
+  c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
+  c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
+  c->i2c_bypass = TRUE;
+  c->drdy_int_enable = FALSE;
+  c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
+}
+
+void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
+{
+  switch (config->init_status) {
+    case MPU60X0_CONF_PWR:
+      /* switch to gyroX clock by default */
+      mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6)));
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_SD:
+      /* configure sample rate divider */
+      mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div);
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_DLPF:
+      /* configure digital low pass filter */
+      mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg);
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_GYRO:
+      /* configure gyro range */
+      mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3));
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_ACCEL:
+      /* configure accelerometer range */
+      mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_I2C_BYPASS:
+      mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_INT_ENABLE:
+      /* configure data ready interrupt */
+      mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0));
+      config->init_status++;
+      break;
+    case MPU60X0_CONF_DONE:
+      config->initialized = TRUE;
+      break;
+    default:
+      break;
+  }
+}
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
index a51b107957..60334d8bd2 100644
--- a/sw/airborne/peripherals/mpu60x0.h
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -50,10 +50,10 @@ enum Mpu60x0ConfStatus {
   MPU60X0_CONF_UNINIT,
   MPU60X0_CONF_PWR,
   MPU60X0_CONF_SD,
-  MPU60X0_CONF_CONFIG,
+  MPU60X0_CONF_DLPF,
   MPU60X0_CONF_GYRO,
   MPU60X0_CONF_ACCEL,
-  MPU60X0_CONF_INT_PIN,
+  MPU60X0_CONF_I2C_BYPASS,
   MPU60X0_CONF_INT_ENABLE,
   MPU60X0_CONF_DONE
 };
@@ -70,59 +70,12 @@ struct Mpu60x0Config {
   bool_t initialized;                   ///< config done flag
 };
 
-static inline void mpu60x0_set_default_config(struct Mpu60x0Config *c)
-{
-  c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
-  c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
-  c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
-  c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
-  c->i2c_bypass = TRUE;
-  c->drdy_int_enable = FALSE;
-  c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
-}
+extern void mpu60x0_set_default_config(struct Mpu60x0Config *c);
 
 /// Configuration function prototype
 typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
 
 /// Configuration sequence called once before normal use
-static inline void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
-{
-  switch (config->init_status) {
-    case MPU60X0_CONF_SD:
-      mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div);
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_CONFIG:
-      mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg);
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_GYRO:
-      mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3));
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_ACCEL:
-      mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_INT_PIN:
-      mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_INT_ENABLE:
-      mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0));
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_PWR:
-      mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6)));
-      config->init_status++;
-      break;
-    case MPU60X0_CONF_DONE:
-      config->initialized = TRUE;
-      break;
-    default:
-      break;
-  }
-}
-
+extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config);
 
 #endif // MPU60X0_H
diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h
index e1d460490c..211d1729f0 100644
--- a/sw/airborne/peripherals/mpu60x0_regs.h
+++ b/sw/airborne/peripherals/mpu60x0_regs.h
@@ -117,7 +117,11 @@
 // MPU60X0 Definitions
 
 #define MPU60X0_REG_WHO_AM_I        0X75
-#define MPU60X0_WHOAMI_REPLY		0x68
+#define MPU60X0_WHOAMI_REPLY	    0x68
+
+// Bit positions
+#define MPU60X0_I2C_BYPASS_EN       1
+#define MPU60X0_I2C_MST_EN		    5
 
 /** Digital Low Pass Filter Options
  *  DLFP is affecting both gyro and accels,