diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c b/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c
index 10994d5948..57c0ea103d 100644
--- a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c
+++ b/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c
@@ -5,7 +5,7 @@
  * Utah State University, http://aggieair.usu.edu/
  */
 #include "subsystems/sensors/baro.h"
-#include "peripherals/ms5611.h"
+#include "peripherals/ms5611_regs.h"
 #include "led.h"
 #include "std.h"
 #include "mcu_periph/sys_time.h"
@@ -15,6 +15,14 @@
 #define MS5611_I2C_DEV i2c2
 #endif
 
+/* default i2c address
+ * when CSB is set to GND addr is 0xEE
+ * when CSB is set to VCC addr is 0xEC
+ *
+ * Note: Aspirin 2.1 has CSB bound to GND.
+ */
+#define MS5611_SLAVE_ADDR 0xEE
+
 #ifdef DEBUG
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c b/sw/airborne/boards/lisa_m/baro_ms5611_spi.c
index bd437cd318..4e58c10605 100644
--- a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c
+++ b/sw/airborne/boards/lisa_m/baro_ms5611_spi.c
@@ -5,7 +5,7 @@
  * Utah State University, http://aggieair.usu.edu/
  */
 #include "subsystems/sensors/baro.h"
-#include "peripherals/ms5611.h"
+#include "peripherals/ms5611_regs.h"
 #include "led.h"
 #include "std.h"
 #include "mcu_periph/sys_time.h"
@@ -14,6 +14,15 @@
 #ifndef MS5611_SPI_DEV
 #define MS5611_SPI_DEV spi2
 #endif
+
+/* SPI SLAVE3 is on pin PC13
+ * Aspirin 2.2 has ms5611 on SPI bus
+ */
+#ifndef MS5611_SLAVE_DEV
+#define MS5611_SLAVE_DEV SPI_SLAVE3
+#endif
+
+
 #define MS5611_BUFFER_LENGTH    4
 
 #ifdef DEBUG
diff --git a/sw/airborne/peripherals/ms5611.c b/sw/airborne/peripherals/ms5611.c
new file mode 100644
index 0000000000..9956d6583b
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
+ * 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/ms5611.c
+ *
+ * MS5611 barometer driver common functions (I2C and SPI).
+ */
+
+#include "peripherals/ms5611.h"
+#include "std.h"
+
+/**
+ * Check if CRC of PROM data is OK.
+ * @return TRUE if OK, FALSE otherwise
+ */
+bool_t ms5611_prom_crc_ok(uint16_t* prom) {
+  int32_t i, j;
+  uint32_t res = 0;
+  uint8_t crc = prom[7] & 0xF;
+  prom[7] &= 0xFF00;
+  for (i = 0; i < 16; i++) {
+    if (i & 1)
+      res ^= ((prom[i>>1]) & 0x00FF);
+    else
+      res ^= (prom[i>>1]>>8);
+    for (j = 8; j > 0; j--) {
+      if (res & 0x8000)
+        res ^= 0x1800;
+      res <<= 1;
+    }
+  }
+  prom[7] |= crc;
+  if (crc == ((res >> 12) & 0xF))
+    return TRUE;
+  else
+    return FALSE;
+}
+
+/**
+ * Calculate temperature and compensated pressure.
+ */
+void ms5611_calc(struct Ms5611Data* ms) {
+  int64_t dt, tempms, off, sens, t2, off2, sens2;
+
+  /* difference between actual and ref temperature */
+  dt = ms->d2 - (int64_t)ms->c[5] * (1<<8);
+  /* actual temperature */
+  tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1<<23);
+  /* offset at actual temperature */
+  off = ((int64_t)ms->c[2] * (1<<16)) + ((int64_t)ms->c[4] * dt) / (1<<7);
+  /* sensitivity at actual temperature */
+  sens = ((int64_t)ms->c[1] * (1<<15)) + ((int64_t)ms->c[3] * dt) / (1<<8);
+  /* second order temperature compensation */
+  if (tempms < 2000) {
+    t2 = (dt*dt) / (1<<31);
+    off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
+    sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
+    if (tempms < -1500) {
+      off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
+      sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
+    }
+    tempms = tempms - t2;
+    off = off - off2;
+    sens = sens - sens2;
+  }
+
+  /* temperature in deg Celsius with 0.01 degC resolultion */
+  ms->temperature = (int32_t)tempms;
+  /* temperature compensated pressure in Pascal (0.01mbar) */
+  ms->pressure = (uint32_t)((((int64_t)ms->d1 * sens) / (1<<21) - off) / (1<<15));
+}
diff --git a/sw/airborne/peripherals/ms5611.h b/sw/airborne/peripherals/ms5611.h
index ad3054e776..85958d5ac5 100644
--- a/sw/airborne/peripherals/ms5611.h
+++ b/sw/airborne/peripherals/ms5611.h
@@ -1,6 +1,5 @@
 /*
- *
- * Copyright (C) 2012 Piotr Esden-Tempski
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
  *
  * This file is part of paparazzi.
  *
@@ -20,82 +19,44 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/* Register definition for MS5611
+/**
+ * @file peripherals/ms5611.h
+ *
+ * MS5611 barometer driver common interface (I2C and SPI).
  */
 
 #ifndef MS5611_H
 #define MS5611_H
 
-/* default i2c address
- * when CSB is set to GND addr is 0xEE
- * when CSB is set to VCC addr is 0xEC
- *
- * Note: Aspirin 2.1 has CSB bound to GND.
- */
-#define MS5611_SLAVE_ADDR 0xEE
+#include "std.h"
 
-/* FIXME: For backwards compatibility with Aspirin driver (it doesnt talk to baro either) */
-#define MS5611_ADDR0 0x77
-#define MS5611_ADDR1 0x76
+/* Include address and register definition */
+#include "peripherals/ms5611_regs.h"
 
-/* SPI SLAVE3 is on pin PC13
- * Aspirin 2.2 has ms5611 on SPI bus
- */
-#ifndef MS5611_SLAVE_DEV
-#define MS5611_SLAVE_DEV SPI_SLAVE3
-#endif
 
-/* Number of 16bit calibration coefficients */
-#define PROM_NB                 8
-
-/* OSR definitions */
-#define MS5611_OSR256        0x02
-#define MS5611_OSR512        0x02
-#define MS5611_OSR1024       0x04
-#define MS5611_OSR2048       0x06
-#define MS5611_OSR4096       0x08
-
-/* D1 Register defines */
-#define MS5611_REG_D1R       0x40 // Request D1 (pressure) conversion
-#define MS5611_REG_D1(_osr)  (MS5611_REG_D1R | _osr)
-#define MS5611_REG_D1OSR256  MS5611_REG_D1(MS5611_ORS256)
-#define MS5611_REG_D1OSR512  MS5611_REG_D1(MS5611_OSR512)
-#define MS5611_REG_D1OSR1024 MS5611_REG_D1(MS5611_OSR1024)
-#define MS5611_REG_D1OSR2048 MS5611_REG_D1(MS5611_OSR2048)
-#define MS5611_REG_D1OSR4096 MS5611_REG_D1(MS5611_OSR4096)
-
-/* D2 register defines */
-#define MS5611_REG_D2R       0x50 // Request D2 (temperature) conversion
-#define MS5611_REG_D2(_osr)  (MS5611_REG_D2R | _osr)
-#define MS5611_REG_D2OSR256  MS5611_REG_D2(MS5611_ORS256)
-#define MS5611_REG_D2OSR512  MS5611_REG_D2(MS5611_OSR512)
-#define MS5611_REG_D2OSR1024 MS5611_REG_D2(MS5611_OSR1024)
-#define MS5611_REG_D2OSR2048 MS5611_REG_D2(MS5611_OSR2048)
-#define MS5611_REG_D2OSR4096 MS5611_REG_D2(MS5611_OSR4096)
-
-/* Commands */
-#define MS5611_ADC_READ         0x00 // Read converted value
-#define MS5611_SOFT_RESET       0x1E // Reset command
-#define MS5611_PROM_READ        0xA0 // Start reading PROM
-#define MS5611_START_CONV_D1    MS5611_REG_D1OSR4096 /* we use OSR=4096 for maximum resolution */
-#define MS5611_START_CONV_D2    MS5611_REG_D2OSR4096 /* we use OSR=4096 for maximum resolution */
-
-/* FIXME: backwards compatibility with Aspirin driver */
-#define MS5611_REG_RESET MS5611_SOFT_RESET
-#define MS5611_REG_ADCREAD MS5611_ADC_READ
-
-enum ms5611_stat{
-  MS5611_UNINIT,
-  MS5611_RESET,
-  MS5611_RESET_OK,
-  MS5611_PROM,
-  MS5611_IDLE,
-  MS5611_CONV_D1,
-  MS5611_CONV_D1_OK,
-  MS5611_ADC_D1,
-  MS5611_CONV_D2,
-  MS5611_CONV_D2_OK,
-  MS5611_ADC_D2
+enum Ms5611Status {
+  MS5611_STATUS_UNINIT,
+  MS5611_STATUS_RESET,
+  MS5611_STATUS_RESET_OK,
+  MS5611_STATUS_PROM,
+  MS5611_STATUS_IDLE,
+  MS5611_STATUS_CONV_D1,
+  MS5611_STATUS_CONV_D1_OK,
+  MS5611_STATUS_ADC_D1,
+  MS5611_STATUS_CONV_D2,
+  MS5611_STATUS_CONV_D2_OK,
+  MS5611_STATUS_ADC_D2
 };
 
+struct Ms5611Data {
+  uint32_t pressure;    ///< pressure in Pascal (0.01mbar)
+  int32_t temperature;  ///< temperature with 0.01 degrees Celsius resolution
+  uint16_t c[PROM_NB];
+  uint32_t d1;
+  uint32_t d2;
+};
+
+extern bool_t ms5611_prom_crc_ok(uint16_t* prom);
+extern void ms5611_calc(struct Ms5611Data* ms);
+
 #endif /* MS5611_H */
diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c
new file mode 100644
index 0000000000..4b248ffe12
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_i2c.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
+ * 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/ms5611_i2c.c
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
+ *
+ */
+
+
+#include "peripherals/ms5611_i2c.h"
+
+
+void ms5611_i2c_init(struct Ms5611_I2c *ms, struct i2c_periph *i2c_p, uint8_t addr)
+{
+  /* set i2c_peripheral */
+  ms->i2c_p = i2c_p;
+
+  /* slave address */
+  ms->i2c_trans.slave_addr = addr;
+  /* set inital status: Success or Done */
+  ms->i2c_trans.status = I2CTransDone;
+
+  ms->data_available = FALSE;
+  ms->initialized = FALSE;
+  ms->status = MS5611_STATUS_UNINIT;
+  ms->prom_cnt = 0;
+}
+
+void ms5611_i2c_start_configure(struct Ms5611_I2c *ms)
+{
+  if (ms->status == MS5611_STATUS_UNINIT) {
+    ms->i2c_trans.buf[0] = MS5611_SOFT_RESET;
+    i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
+    ms->status = MS5611_STATUS_RESET;
+  }
+}
+
+void ms5611_i2c_start_conversion(struct Ms5611_I2c *ms)
+{
+  if (ms->status == MS5611_STATUS_IDLE &&
+      ms->i2c_trans.status == I2CTransDone) {
+    /* start D1 conversion */
+    ms->i2c_trans.buf[0] = MS5611_START_CONV_D1;
+    i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
+    ms->status = MS5611_STATUS_CONV_D1;
+  }
+}
+
+/**
+ * Periodic function to ensure proper delay after triggering reset or conversion.
+ * Should run at 100Hz max.
+ * Typical conversion time is 8.22ms at max resolution.
+ */
+void ms5611_i2c_periodic_check(struct Ms5611_I2c *ms)
+{
+  switch (ms->status) {
+    case MS5611_STATUS_RESET:
+      ms->status = MS5611_STATUS_RESET_OK;
+      break;
+    case MS5611_STATUS_RESET_OK:
+      if (ms->i2c_trans.status == I2CTransDone) {
+        /* start getting prom data */
+        ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+        i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
+        ms->status = MS5611_STATUS_PROM;
+      }
+      break;
+    case MS5611_STATUS_CONV_D1:
+      ms->status = MS5611_STATUS_CONV_D1_OK;
+      break;
+    case MS5611_STATUS_CONV_D1_OK:
+      if (ms->i2c_trans.status == I2CTransDone) {
+        /* read D1 adc */
+        ms->i2c_trans.buf[0] = MS5611_ADC_READ;
+        i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
+        ms->status = MS5611_STATUS_ADC_D1;
+      }
+      break;
+    case MS5611_STATUS_CONV_D2:
+      ms->status = MS5611_STATUS_CONV_D2_OK;
+      break;
+    case MS5611_STATUS_CONV_D2_OK:
+      if (ms->i2c_trans.status == I2CTransDone) {
+        /* read D2 adc */
+        ms->i2c_trans.buf[0] = MS5611_ADC_READ;
+        i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
+        ms->status = MS5611_STATUS_ADC_D2;
+      }
+      break;
+    default:
+      break;
+  }
+}
+
+void ms5611_i2c_event(struct Ms5611_I2c *ms) {
+  if (ms->initialized) {
+    if (ms->i2c_trans.status == I2CTransFailed) {
+      ms->i2c_trans.status = I2CTransDone;
+    }
+    else if (ms->i2c_trans.status == I2CTransSuccess) {
+      // Successfull reading
+      switch (ms->status) {
+
+        case MS5611_STATUS_PROM:
+          /* read prom data */
+          ms->data.c[ms->prom_cnt++] = (ms->i2c_trans.buf[0] << 8) |
+                                             ms->i2c_trans.buf[1];
+          if (ms->prom_cnt < PROM_NB) {
+            /* get next prom data */
+            ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+            i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
+          }
+          else {
+            /* done reading prom, check prom crc */
+            if (ms5611_prom_crc_ok(ms->data.c)) {
+              ms->initialized = TRUE;
+              ms->status = MS5611_STATUS_IDLE;
+            }
+            else {
+              /* checksum error, try again */
+              ms->prom_cnt = 0;
+              ms->status = MS5611_STATUS_UNINIT;
+            }
+          }
+          break;
+
+        case MS5611_STATUS_ADC_D1:
+          /* read D1 (pressure) */
+          ms->data.d1 = (ms->i2c_trans.buf[0] << 16) |
+                        (ms->i2c_trans.buf[1] << 8) |
+                         ms->i2c_trans.buf[2];
+          /* start D2 conversion */
+          ms->i2c_trans.buf[0] = MS5611_START_CONV_D2;
+          i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
+          ms->status = MS5611_STATUS_CONV_D2;
+          break;
+
+        case MS5611_STATUS_ADC_D2:
+          /* read D2 (temperature) */
+          ms->data.d2 = (ms->i2c_trans.buf[0] << 16) |
+                        (ms->i2c_trans.buf[1] << 8) |
+                         ms->i2c_trans.buf[2];
+          /* calculate temp and pressure from measurements */
+          ms5611_calc(&(ms->data));
+          ms->status = MS5611_STATUS_IDLE;
+          ms->data_available = TRUE;
+          break;
+
+        default:
+          break;
+      }
+      ms->i2c_trans.status = I2CTransDone;
+    }
+  }
+  else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
+    switch (ms->i2c_trans.status) {
+      case I2CTransFailed:
+        /* try again */
+        ms->prom_cnt = 0;
+        ms->status = MS5611_STATUS_UNINIT;
+      case I2CTransSuccess:
+      case I2CTransDone:
+        ms->i2c_trans.status = I2CTransDone;
+        break;
+      default:
+        break;
+    }
+  }
+}
diff --git a/sw/airborne/peripherals/ms5611_i2c.h b/sw/airborne/peripherals/ms5611_i2c.h
new file mode 100644
index 0000000000..614e3c605b
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_i2c.h
@@ -0,0 +1,63 @@
+/*
+ * 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/ms5611_i2c.h
+ *
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
+ */
+
+#ifndef MS5611_I2C_H
+#define MS5611_I2C_H
+
+#include "mcu_periph/i2c.h"
+
+/* Include common MS5611 definitions */
+#include "peripherals/ms5611.h"
+
+struct Ms5611_I2c {
+  struct i2c_periph *i2c_p;
+  struct i2c_transaction i2c_trans;
+  enum Ms5611Status status;
+  bool_t initialized;                 ///< config done flag
+  volatile bool_t data_available;     ///< data ready flag
+  struct Ms5611Data data;
+  int32_t prom_cnt;                   ///< number of bytes read from PROM
+};
+
+// Functions
+extern void ms5611_i2c_init(struct Ms5611_I2c* ms, struct i2c_periph* i2c_p, uint8_t addr);
+extern void ms5611_i2c_start_configure(struct Ms5611_I2c* ms);
+extern void ms5611_i2c_start_conversion(struct Ms5611_I2c* ms);
+extern void ms5611_i2c_periodic_check(struct Ms5611_I2c* ms);
+extern void ms5611_i2c_event(struct Ms5611_I2c* ms);
+
+/// convenience function: trigger read or start configuration if not already initialized
+static inline void ms5611_i2c_periodic(struct Ms5611_I2c* ms) {
+  if (ms->initialized)
+    ms5611_i2c_start_conversion(ms);
+  else
+    ms5611_i2c_start_configure(ms);
+  ms5611_i2c_periodic_check(ms);
+}
+
+
+#endif /* MS5611_I2C_H */
diff --git a/sw/airborne/peripherals/ms5611_regs.h b/sw/airborne/peripherals/ms5611_regs.h
new file mode 100644
index 0000000000..6cd3eb1fe2
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_regs.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2012 Piotr Esden-Tempski
+ *
+ * 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/ms5611_regs.h
+ * Register definitions for MS5611 barometer.
+ */
+
+#ifndef MS5611_REGS_H
+#define MS5611_REGS_H
+
+/** default i2c address
+ * when CSB is set to GND addr is 0xEE
+ * when CSB is set to VCC addr is 0xEC
+ */
+#define MS5611_I2C_SLAVE_ADDR 0xEE
+#define MS5611_I2C_SLAVE_ADDR_ALT 0xEC
+
+/* Number of 16bit calibration coefficients */
+#define PROM_NB                 8
+
+/* OSR definitions */
+#define MS5611_OSR256        0x02
+#define MS5611_OSR512        0x02
+#define MS5611_OSR1024       0x04
+#define MS5611_OSR2048       0x06
+#define MS5611_OSR4096       0x08
+
+/* D1 Register defines */
+#define MS5611_REG_D1R       0x40 // Request D1 (pressure) conversion
+#define MS5611_REG_D1(_osr)  (MS5611_REG_D1R | _osr)
+#define MS5611_REG_D1OSR256  MS5611_REG_D1(MS5611_ORS256)
+#define MS5611_REG_D1OSR512  MS5611_REG_D1(MS5611_OSR512)
+#define MS5611_REG_D1OSR1024 MS5611_REG_D1(MS5611_OSR1024)
+#define MS5611_REG_D1OSR2048 MS5611_REG_D1(MS5611_OSR2048)
+#define MS5611_REG_D1OSR4096 MS5611_REG_D1(MS5611_OSR4096)
+
+/* D2 register defines */
+#define MS5611_REG_D2R       0x50 // Request D2 (temperature) conversion
+#define MS5611_REG_D2(_osr)  (MS5611_REG_D2R | _osr)
+#define MS5611_REG_D2OSR256  MS5611_REG_D2(MS5611_ORS256)
+#define MS5611_REG_D2OSR512  MS5611_REG_D2(MS5611_OSR512)
+#define MS5611_REG_D2OSR1024 MS5611_REG_D2(MS5611_OSR1024)
+#define MS5611_REG_D2OSR2048 MS5611_REG_D2(MS5611_OSR2048)
+#define MS5611_REG_D2OSR4096 MS5611_REG_D2(MS5611_OSR4096)
+
+/* Commands */
+#define MS5611_ADC_READ         0x00 // Read converted value
+#define MS5611_SOFT_RESET       0x1E // Reset command
+#define MS5611_PROM_READ        0xA0 // Start reading PROM
+#define MS5611_START_CONV_D1    MS5611_REG_D1OSR4096 /* we use OSR=4096 for maximum resolution */
+#define MS5611_START_CONV_D2    MS5611_REG_D2OSR4096 /* we use OSR=4096 for maximum resolution */
+
+#endif /* MS5611_REGS_H */
diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c
new file mode 100644
index 0000000000..bcf7d129d4
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_spi.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
+ * 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/ms5611_spi.c
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
+ *
+ */
+
+
+#include "peripherals/ms5611_spi.h"
+
+
+void ms5611_spi_init(struct Ms5611_Spi *ms, struct spi_periph *spi_p, uint8_t slave_idx)
+{
+  /* set spi_peripheral */
+  ms->spi_p = spi_p;
+
+  /* configure spi transaction */
+  ms->spi_trans.cpol = SPICpolIdleHigh;
+  ms->spi_trans.cpha = SPICphaEdge2;
+  ms->spi_trans.dss = SPIDss8bit;
+  ms->spi_trans.bitorder = SPIMSBFirst;
+  ms->spi_trans.cdiv = SPIDiv64;
+
+  ms->spi_trans.select = SPISelectUnselect;
+  ms->spi_trans.slave_idx = slave_idx;
+  ms->spi_trans.output_length = 1;
+  ms->spi_trans.input_length = 4;
+  ms->spi_trans.before_cb = NULL;
+  ms->spi_trans.after_cb = NULL;
+  ms->spi_trans.input_buf = ms->rx_buf;
+  ms->spi_trans.output_buf = ms->tx_buf;
+
+  /* set inital status: Success or Done */
+  ms->spi_trans.status = SPITransDone;
+
+  ms->data_available = FALSE;
+  ms->initialized = FALSE;
+  ms->status = MS5611_STATUS_UNINIT;
+  ms->prom_cnt = 0;
+}
+
+void ms5611_spi_start_configure(struct Ms5611_Spi *ms)
+{
+  if (ms->status == MS5611_STATUS_UNINIT) {
+    ms->tx_buf[0] = MS5611_SOFT_RESET;
+    spi_submit(ms->spi_p, &(ms->spi_trans));
+    ms->status = MS5611_STATUS_RESET;
+  }
+}
+
+void ms5611_spi_start_conversion(struct Ms5611_Spi *ms)
+{
+  if (ms->status == MS5611_STATUS_IDLE &&
+      ms->spi_trans.status == SPITransDone) {
+    /* start D1 conversion */
+    ms->tx_buf[0] = MS5611_START_CONV_D1;
+    spi_submit(ms->spi_p, &(ms->spi_trans));
+    ms->status = MS5611_STATUS_CONV_D1;
+  }
+}
+
+/**
+ * Periodic function to ensure proper delay after triggering reset or conversion.
+ * Should run at 100Hz max.
+ * Typical conversion time is 8.22ms at max resolution.
+ */
+void ms5611_spi_periodic_check(struct Ms5611_Spi *ms)
+{
+  switch (ms->status) {
+    case MS5611_STATUS_RESET:
+      ms->status = MS5611_STATUS_RESET_OK;
+      break;
+    case MS5611_STATUS_RESET_OK:
+      if (ms->spi_trans.status == SPITransDone) {
+        /* start getting prom data */
+        ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+        spi_submit(ms->spi_p, &(ms->spi_trans));
+        ms->status = MS5611_STATUS_PROM;
+      }
+      break;
+    case MS5611_STATUS_CONV_D1:
+      ms->status = MS5611_STATUS_CONV_D1_OK;
+      break;
+    case MS5611_STATUS_CONV_D1_OK:
+      if (ms->spi_trans.status == SPITransDone) {
+        /* read D1 adc */
+        ms->tx_buf[0] = MS5611_ADC_READ;
+        spi_submit(ms->spi_p, &(ms->spi_trans));
+        ms->status = MS5611_STATUS_ADC_D1;
+      }
+      break;
+    case MS5611_STATUS_CONV_D2:
+      ms->status = MS5611_STATUS_CONV_D2_OK;
+      break;
+    case MS5611_STATUS_CONV_D2_OK:
+      if (ms->spi_trans.status == SPITransDone) {
+        /* read D2 adc */
+        ms->tx_buf[0] = MS5611_ADC_READ;
+        spi_submit(ms->spi_p, &(ms->spi_trans));
+        ms->status = MS5611_STATUS_ADC_D2;
+      }
+      break;
+    default:
+      break;
+  }
+}
+
+void ms5611_spi_event(struct Ms5611_Spi *ms) {
+  if (ms->initialized) {
+    if (ms->spi_trans.status == SPITransFailed) {
+      ms->spi_trans.status = SPITransDone;
+    }
+    else if (ms->spi_trans.status == SPITransSuccess) {
+      // Successfull reading
+      switch (ms->status) {
+
+        case MS5611_STATUS_PROM:
+          /* read prom data */
+          ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) |
+                                        ms->rx_buf[2];
+          if (ms->prom_cnt < PROM_NB) {
+            /* get next prom data */
+            ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+            spi_submit(ms->spi_p, &(ms->spi_trans));
+          }
+          else {
+            /* done reading prom, check prom crc */
+            if (ms5611_prom_crc_ok(ms->data.c)) {
+              ms->initialized = TRUE;
+              ms->status = MS5611_STATUS_IDLE;
+            }
+            else {
+              /* checksum error, try again */
+              ms->prom_cnt = 0;
+              ms->status = MS5611_STATUS_UNINIT;
+            }
+          }
+          break;
+
+        case MS5611_STATUS_ADC_D1:
+          /* read D1 (pressure) */
+          ms->data.d1 = (ms->rx_buf[1] << 16) |
+                        (ms->rx_buf[2] << 8) |
+                         ms->rx_buf[3];
+          /* start D2 conversion */
+          ms->tx_buf[0] = MS5611_START_CONV_D2;
+          spi_submit(ms->spi_p, &(ms->spi_trans));
+          ms->status = MS5611_STATUS_CONV_D2;
+          break;
+
+        case MS5611_STATUS_ADC_D2:
+          /* read D2 (temperature) */
+          ms->data.d2 = (ms->rx_buf[1] << 16) |
+                        (ms->rx_buf[2] << 8) |
+                         ms->rx_buf[3];
+          /* calculate temp and pressure from measurements */
+          ms5611_calc(&(ms->data));
+          ms->status = MS5611_STATUS_IDLE;
+          ms->data_available = TRUE;
+          break;
+
+        default:
+          break;
+      }
+      ms->spi_trans.status = SPITransDone;
+    }
+  }
+  else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
+    switch (ms->spi_trans.status) {
+      case SPITransFailed:
+        /* try again */
+        ms->prom_cnt = 0;
+        ms->status = MS5611_STATUS_UNINIT;
+      case SPITransSuccess:
+      case SPITransDone:
+        ms->spi_trans.status = SPITransDone;
+        break;
+      default:
+        break;
+    }
+  }
+}
diff --git a/sw/airborne/peripherals/ms5611_spi.h b/sw/airborne/peripherals/ms5611_spi.h
new file mode 100644
index 0000000000..8e0b42c1b2
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_spi.h
@@ -0,0 +1,65 @@
+/*
+ * 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/ms5611_spi.h
+ *
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
+ */
+
+#ifndef MS5611_SPI_H
+#define MS5611_SPI_H
+
+#include "mcu_periph/spi.h"
+
+/* Include common MS5611 definitions */
+#include "peripherals/ms5611.h"
+
+struct Ms5611_Spi {
+  struct spi_periph *spi_p;
+  struct spi_transaction spi_trans;
+  volatile uint8_t tx_buf[1];
+  volatile uint8_t rx_buf[4];
+  enum Ms5611Status status;
+  bool_t initialized;                 ///< config done flag
+  volatile bool_t data_available;     ///< data ready flag
+  struct Ms5611Data data;
+  int32_t prom_cnt;                   ///< number of bytes read from PROM
+};
+
+// Functions
+extern void ms5611_spi_init(struct Ms5611_Spi* ms, struct spi_periph* spi_p, uint8_t addr);
+extern void ms5611_spi_start_configure(struct Ms5611_Spi* ms);
+extern void ms5611_spi_start_conversion(struct Ms5611_Spi* ms);
+extern void ms5611_spi_periodic_check(struct Ms5611_Spi* ms);
+extern void ms5611_spi_event(struct Ms5611_Spi* ms);
+
+/// convenience function: trigger read or start configuration if not already initialized
+static inline void ms5611_spi_periodic(struct Ms5611_Spi* ms) {
+  if (ms->initialized)
+    ms5611_spi_start_conversion(ms);
+  else
+    ms5611_spi_start_configure(ms);
+  ms5611_spi_periodic_check(ms);
+}
+
+
+#endif /* MS5611_SPI_H */