diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c
index 4d87ea5003..8e686c004e 100644
--- a/sw/airborne/boards/lisa_m/baro_board.c
+++ b/sw/airborne/boards/lisa_m/baro_board.c
@@ -1,26 +1,16 @@
 
 #include "subsystems/sensors/baro.h"
+#include <stm32/gpio.h>
 
 struct Baro baro;
 struct BaroBoard baro_board;
 struct i2c_transaction baro_trans;
-
-
-static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb);
-static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr);
-static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr);
-static inline void baro_board_read(void);
-
+struct bmp085_baro_calibration calibration;
 
 #define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3)
 #define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1)
 
-void baro_init(void) {
-  baro.status = BS_UNINITIALIZED;
-  baro.absolute     = 0;
-  baro.differential = 0;
-  baro_board.status = LBS_UNINITIALIZED;
-}
+// FIXME: BARO DRDY connected to PB0 for lisa/m
 
 static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
 {
@@ -33,56 +23,23 @@ static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
   while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
 }
 
-static inline void bmp085_request_pressure(void)
+static inline void bmp085_read_reg16(uint8_t addr)
 {
-  bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6));
-}
-
-
-void baro_periodic(void) {
-  // check i2c_done
-  if (!i2c_idle(&i2c2)) return;
-  switch (baro_board.status) {
-  case LBS_UNINITIALIZED:
-    baro_board_send_reset();
-    baro_board.status = LBS_RESETED;
-    break;
-  case LBS_RESETED:
-    baro_board_send_config();
-    baro_board.status = LBS_INITIALIZING;
-    break;
-  case LBS_INITIALIZING:
-    baro_board_set_current_register(BMP085_ADDR, 0x00);
-    baro_board.status = LBS_INITIALIZING_1;
-    break;
-  case LBS_INITIALIZING_1:
-    baro.status = BS_RUNNING;
-  case LBS_REQUEST:
-    bmp085_request_pressure();
-    baro_board.status = LBS_READ;
-    break;
-  case LBS_READ:
-    baro_board_read();
-    baro_board.status = LBS_READING;
-    break;
-  default:
-    break;
-  }
-
-}
-
-
-void baro_board_send_config(void) {
-  /* maybe we should read factory calibration here */
-  //baro_board_write_to_register(BMP085_ADDR, 0x01, 0x86, 0x83);
-}
-
-void baro_board_send_reset(void) {
-  baro_trans.type = I2CTransTx;
-  baro_trans.slave_addr = 0x00;
+  baro_trans.type = I2CTransTxRx;
+  baro_trans.slave_addr = BMP085_ADDR;
   baro_trans.len_w = 1;
-  baro_trans.buf[0] = 0x06;
-  i2c_submit(&i2c2,&baro_trans);
+  baro_trans.len_r = 2;
+  baro_trans.buf[0] = addr;
+  i2c_submit(&i2c2, &baro_trans);
+}
+
+static inline int16_t bmp085_read_reg16_blocking(uint8_t addr)
+{
+  bmp085_read_reg16(addr);
+
+  while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
+
+  return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]);
 }
 
 static inline void bmp085_read_reg24(uint8_t addr)
@@ -93,51 +50,101 @@ static inline void bmp085_read_reg24(uint8_t addr)
   baro_trans.len_r = 3;
   baro_trans.buf[0] = addr;
   i2c_submit(&i2c2, &baro_trans);
-  //while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
-
-  //return (baro_trans.buf[0] << 16) | (baro_trans.buf[1] >> 8) | (baro_trans.buf[2]);
 }
 
-static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) {
-  baro_trans.type = I2CTransTx;
-  baro_trans.slave_addr = baro_addr;
-  baro_trans.len_w = 3;
-  baro_trans.buf[0] = reg_addr;
-  baro_trans.buf[1] = val_msb;
-  baro_trans.buf[2] = val_lsb;
-  i2c_submit(&i2c2,&baro_trans);
+static void bmp085_baro_read_calibration(void)
+{
+  calibration.ac1 = bmp085_read_reg16_blocking(0xAA); // AC1
+  calibration.ac2 = bmp085_read_reg16_blocking(0xAC); // AC2
+  calibration.ac3 = bmp085_read_reg16_blocking(0xAE); // AC3
+  calibration.ac4 = bmp085_read_reg16_blocking(0xB0); // AC4
+  calibration.ac5 = bmp085_read_reg16_blocking(0xB2); // AC5
+  calibration.ac6 = bmp085_read_reg16_blocking(0xB4); // AC6
+  calibration.b1 = bmp085_read_reg16_blocking(0xB6); // B1
+  calibration.b2 = bmp085_read_reg16_blocking(0xB8); // B2
+  calibration.mb = bmp085_read_reg16_blocking(0xBA); // MB
+  calibration.mc = bmp085_read_reg16_blocking(0xBC); // MC
+  calibration.md = bmp085_read_reg16_blocking(0xBE); // MD
 }
 
-static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) {
-  baro_trans.type = I2CTransTxRx;
-  baro_trans.slave_addr = baro_addr;
-  baro_trans.len_w = 1;
-  baro_trans.len_r = 2;
-  baro_trans.buf[0] = reg_addr;
-  i2c_submit(&i2c2,&baro_trans);
+void baro_init(void) {
+  baro.status = BS_UNINITIALIZED;
+  baro.absolute     = 0;
+  baro.differential = 0;
+  baro_board.status = LBS_UNINITIALIZED;
+  bmp085_baro_read_calibration();
+
+  /* STM32 specific (maybe this is a LISA/M specific driver anyway?) */
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
 }
 
-static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) {
-  baro_trans.type = I2CTransTx;
-  baro_trans.slave_addr = baro_addr;
-  baro_trans.len_w = 1;
-  baro_trans.buf[0] = reg_addr;
-  i2c_submit(&i2c2,&baro_trans);
+static inline int baro_eoc(void)
+{
+  return GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_0);
 }
 
+static inline void bmp085_request_pressure(void)
+{
+  bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6));
+}
+
+static inline void bmp085_request_temp(void)
+{
+  bmp085_write_reg(0xF4, 0x2E);
+}
 
 static inline void bmp085_read_pressure(void)
 {
   bmp085_read_reg24(0xF6);
 }
 
-static inline void baro_board_read()
+static inline void bmp085_read_temp(void)
 {
-  //int32_t x;
-  //bmp085_request_pressure();
-  bmp085_read_pressure();
-  //baro_trans.type = I2CTransRx;
-  //baro_trans.slave_addr = BMP085_ADDR;
-  //baro_trans.len_r = 2;
-  //i2c_submit(&i2c2,&baro_trans);
+  bmp085_read_reg16(0xF6);
+}
+
+void baro_periodic(void) {
+  // check that nothing is in progress
+  if (baro_trans.status == I2CTransPending) return;
+  if (baro_trans.status == I2CTransRunning) return;
+  if (!i2c_idle(&i2c2)) return;
+
+  switch (baro_board.status) {
+  case LBS_UNINITIALIZED:
+    baro_board_send_reset();
+    baro_board.status = LBS_REQUEST;
+    baro.status = BS_RUNNING;
+    break;
+  case LBS_REQUEST:
+    bmp085_request_pressure();
+    baro_board.status = LBS_READ;
+    break;
+  case LBS_READ:
+    if (baro_eoc()) {
+      bmp085_read_pressure();
+      baro_board.status = LBS_READING;
+    }
+    break;
+  case LBS_REQUEST_TEMP:
+    bmp085_request_temp();
+    baro_board.status = LBS_READ_TEMP;
+    break;
+  case LBS_READ_TEMP:
+    if (baro_eoc()) {
+      bmp085_read_temp();
+      baro_board.status = LBS_READING_TEMP;
+    }
+    break;
+  default:
+    break;
+  }
+
+}
+
+void baro_board_send_reset(void) {
+  // This is a NOP at the moment
 }
diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h
index 374f9d30ae..cbe3454983 100644
--- a/sw/airborne/boards/lisa_m/baro_board.h
+++ b/sw/airborne/boards/lisa_m/baro_board.h
@@ -10,44 +10,97 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
-// absolute
+// absolute addr
 #define BMP085_ADDR  0xEE
+// Over sample setting (0-3)
 #define BMP085_OSS 3
 
 enum LisaBaroStatus {
   LBS_UNINITIALIZED,
-  LBS_RESETED,
-  LBS_INITIALIZING,
-  LBS_INITIALIZING_1,
-  LBS_IDLE,
   LBS_REQUEST,
   LBS_READING,
   LBS_READ,
+  LBS_REQUEST_TEMP,
+  LBS_READING_TEMP,
+  LBS_READ_TEMP,
 };
 
 struct BaroBoard {
   enum LisaBaroStatus status;
 };
 
+struct bmp085_baro_calibration {
+  // These values come from EEPROM on sensor
+  int16_t ac1;
+  int16_t ac2;
+  int16_t ac3;
+  uint16_t ac4;
+  uint16_t ac5;
+  uint16_t ac6;
+  int16_t b1;
+  int16_t b2;
+  int16_t mb;
+  int16_t mc;
+  int16_t md;
+  
+  // These values are calculated
+  int32_t b5;
+};
+
 extern struct BaroBoard baro_board;
 extern struct i2c_transaction baro_trans;
+extern struct bmp085_baro_calibration calibration;
 
 extern void baro_board_send_reset(void);
 extern void baro_board_send_config(void);
 
-#define BaroEvent(_b_abs_handler, _b_diff_handler) {			\
-    if (baro_board.status == LBS_READING &&				\
-	baro_trans.status != I2CTransPending) {				\
-      baro_board.status = LBS_REQUEST;					\
-			if (baro_trans.status == I2CTransSuccess) { \
-				int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[0];		\
-				baro.absolute = tmp >> ( 8 - BMP085_OSS);						\
-				_b_abs_handler();							\
-			} \
-    }									\
+// Apply temp calibration and sensor calibration to raw measurement to get Pa (from BMP085 datasheet)
+static inline int32_t baro_apply_calibration(int32_t raw)
+{
+  int32_t b6 = calibration.b5 - 4000;
+  int x1 = (calibration.b2 * (b6 * b6 >> 12)) >> 11;
+  int x2 = calibration.ac2 * b6 >> 11;
+  int32_t x3 = x1 + x2;
+  int32_t b3 = (((calibration.ac1 * 4 + x3) << BMP085_OSS) + 2)/4;
+  x1 = calibration.ac3 * b6 >> 13;
+  x2 = (calibration.b1 * (b6 * b6 >> 12)) >> 16;
+  x3 = ((x1 + x2) + 2) >> 2;
+  uint32_t b4 = (calibration.ac4 * (uint32_t) (x3 + 32768)) >> 15;
+  uint32_t b7 = (raw - b3) * (50000 >> BMP085_OSS);
+  int32_t p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
+  x1 = (p >> 8) * (p >> 8);
+  x1 = (x1 * 3038) >> 16;
+  x2 = (-7357 * p) >> 16;
+  return p + ((x1 + x2 + 3791) >> 4);
+}
+
+static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
+{
+  if (baro_board.status == LBS_READING &&
+      baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) {
+    baro_board.status = LBS_REQUEST_TEMP;
+    if (baro_trans.status == I2CTransSuccess) {
+      int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[2];
+      tmp = tmp >> (8 - BMP085_OSS);
+      baro.absolute = baro_apply_calibration(tmp);
+      b_abs_handler();
+    }
   }
+  if (baro_board.status == LBS_READING_TEMP &&
+      baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) {
+    baro_board.status = LBS_REQUEST;
+    if (baro_trans.status == I2CTransSuccess) {
+      // abuse differential to store temp in 0.1C for now
+      int32_t tmp = (baro_trans.buf[0] << 8) | baro_trans.buf[1];
+      int32_t x1 = ((tmp - calibration.ac6) * calibration.ac5) >> 15;
+      int32_t x2 = (calibration.mc << 11) / (x1 + calibration.md);
+      calibration.b5 = x1 + x2;
+      baro.differential = (calibration.b5 + 8) >> 4;
+      b_diff_handler();
+    }
+  }
+}
 
-
-
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
 
 #endif /* BOARDS_LISA_M_BARO_H */