diff --git a/sw/airborne/arm7/i2c_hw.c b/sw/airborne/arm7/i2c_hw.c
index 58bb9baace..1f7e54d8eb 100644
--- a/sw/airborne/arm7/i2c_hw.c
+++ b/sw/airborne/arm7/i2c_hw.c
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *  
- * Copyright (C) 2008  Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2010 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -32,6 +32,142 @@
 #include "interrupt_hw.h"
 
 
+///////////////////
+// I2C Automaton //
+///////////////////
+
+static inline void I2cSendStart(struct i2c_periph* p) {
+  p->status = I2CStartRequested;
+  ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STA);
+}
+
+static inline void I2cSendAck(void* reg) {
+  ((i2cRegs_t *)reg)->conset = _BV(AA);
+}
+
+static inline void I2cEndOfTransaction(struct i2c_periph* p) {
+  // handle fifo here
+  p->trans_extract_idx++;
+  if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+    p->trans_extract_idx = 0;
+  // if no more transaction to process, stop here, else start next transaction
+  if (p->trans_extract_idx == p->trans_insert_idx) {
+    p->status = I2CIdle;
+  }
+  else {
+    I2cSendStart(p);
+  }
+}
+
+static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) {
+  // transaction finished with success
+  t->status = I2CTransSuccess;
+  I2cEndOfTransaction(p);
+}
+
+static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) {
+  ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
+  I2cFinished(p,t); 
+}
+
+static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) {
+  ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
+  t->status = I2CTransFailed;
+  p->status = I2CFailed;
+  // FIXME I2C should be reseted here
+  I2cEndOfTransaction(p);
+}
+
+static inline void I2cSendByte(void* reg, uint8_t b) {
+  ((i2cRegs_t *)reg)->dat = b;
+}
+
+static inline void I2cReceive(void* reg, bool_t ack) {
+  if (ack) ((i2cRegs_t *)reg)->conset = _BV(AA);
+  else ((i2cRegs_t *)reg)->conclr = _BV(AAC);
+}
+
+static inline void I2cClearStart(void* reg) {
+  ((i2cRegs_t *)reg)->conclr = _BV(STAC);
+}
+
+static inline void I2cClearIT(void* reg) {
+  ((i2cRegs_t *)reg)->conclr = _BV(SIC);
+}
+
+static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) {
+  struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+  switch (state) {
+    case I2C_START:
+    case I2C_RESTART:
+      // Set R/W flag
+      switch (trans->type) {
+        case I2CTransRx :
+          SetBit(trans->slave_addr,0);
+          break;
+        case I2CTransTx: 
+        case I2CTransTxRx: 
+          ClearBit(trans->slave_addr,0);
+          break;
+      }
+      I2cSendByte(p->reg_addr,trans->slave_addr);
+      I2cClearStart(p->reg_addr);
+      p->idx_buf = 0;
+      break;
+    case I2C_MR_DATA_ACK:
+      if (p->idx_buf < trans->len_r) {
+        trans->buf[p->idx_buf] = ((i2cRegs_t *)(p->reg_addr))->dat;
+        p->idx_buf++;
+        I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1);
+      }
+      else {
+        /* error , we should have got NACK */
+        I2cSendStop(p,trans);
+      }
+      break;
+    case I2C_MR_SLA_ACK: /* At least one char */
+      /* Wait and reply with ACK or NACK */
+      I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1);
+      break;
+    case I2C_MR_SLA_NACK:
+    case I2C_MT_SLA_NACK:
+      I2cSendStart(p);
+      break;
+    case I2C_MT_SLA_ACK:
+    case I2C_MT_DATA_ACK:
+      if (p->idx_buf < trans->len_w) {
+        I2cSendByte(p->reg_addr,trans->buf[p->idx_buf]);
+        p->idx_buf++;
+      } else {
+        if (trans->type == I2CTransTxRx) {
+          trans->type = I2CTransRx;	/* FIXME should not change type */
+          p->idx_buf = 0;
+          trans->slave_addr |= 1;
+          I2cSendStart(p);
+        } else {
+          if (trans->stop_after_transmit) {
+            I2cSendStop(p,trans);
+          } else {
+            I2cFinished(p,trans);
+          }
+        }
+      }
+      break;
+    case I2C_MR_DATA_NACK:
+      if (p->idx_buf < trans->len_r) {
+        trans->buf[p->idx_buf] = ((i2cRegs_t *)(p->reg_addr))->dat;
+      }
+      I2cSendStop(p,trans);
+      break;
+    default:
+      I2cSendStop(p,trans);
+      //I2cFail(p,trans);
+      /* FIXME log error */
+      break;
+  }
+}
+
+
 #ifdef USE_I2C0
 
 /* default clock speed 37.5KHz with our 15MHz PCLK 
@@ -189,3 +325,109 @@ void i2c1_ISR(void)
 
 #endif /* USE_I2C1 */
 
+#ifdef USE_I2C2
+
+
+/* default clock speed 37.5KHz with our 15MHz PCLK 
+   I2C0_CLOCK = PCLK / (I2C0_SCLL + I2C0_SCLH)     */
+#ifndef I2C0_SCLL
+#define I2C0_SCLL 200
+#endif
+
+#ifndef I2C0_SCLH
+#define I2C0_SCLH 200
+#endif
+
+/* adjust for other PCLKs */
+
+#if (PCLK == 15000000)
+#define I2C0_SCLL_D I2C0_SCLL
+#define I2C0_SCLH_D I2C0_SCLH
+#else
+
+#if (PCLK == 30000000)
+#define I2C0_SCLL_D (2*I2C0_SCLL)
+#define I2C0_SCLH_D (2*I2C0_SCLH)
+#else
+
+#if (PCLK == 60000000)
+#define I2C0_SCLL_D (4*I2C0_SCLL)
+#define I2C0_SCLH_D (4*I2C0_SCLH)
+#else
+
+#error unknown PCLK frequency
+#endif
+#endif
+#endif
+
+#ifndef I2C0_VIC_SLOT
+#define I2C0_VIC_SLOT 9
+#endif
+
+void i2c0_ISR(void) __attribute__((naked));
+
+
+/* SDA0 on P0.3 */
+/* SCL0 on P0.2 */
+void i2c2_hw_init ( void ) {
+
+  i2c2.reg_addr = I2C0;
+
+  /* set P0.2 and P0.3 to I2C0 */
+  PINSEL0 |= 1 << 4 | 1 << 6;
+  /* clear all flags */
+  I2C0CONCLR = _BV(AAC) | _BV(SIC) | _BV(STAC) | _BV(I2ENC);
+  /* enable I2C */
+  I2C0CONSET = _BV(I2EN);
+  /* set bitrate */
+  I2C0SCLL = I2C0_SCLL_D;  
+  I2C0SCLH = I2C0_SCLH_D;  
+  
+  // initialize the interrupt vector
+  VICIntSelect &= ~VIC_BIT(VIC_I2C0);              // I2C0 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_I2C0);                // I2C0 interrupt enabled
+  _VIC_CNTL(I2C0_VIC_SLOT) = VIC_ENABLE | VIC_I2C0;
+  _VIC_ADDR(I2C0_VIC_SLOT) = (uint32_t)i2c0_ISR;    // address of the ISR
+}
+
+#define I2C_STATUS_REG I2C0STAT
+
+void i2c0_ISR(void)
+{
+  ISR_ENTRY();
+
+  uint32_t state = I2C_STATUS_REG;
+  I2cAutomaton(state,&i2c2);
+  I2cClearIT(i2c2.reg_addr);
+  
+  VICVectAddr = 0x00000000;             // clear this interrupt from the VIC
+  ISR_EXIT();                           // recover registers and return
+}
+
+
+bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
+
+  uint8_t idx;
+  idx = p->trans_insert_idx + 1;
+  if (idx >= I2C_TRANSACTION_QUEUE_LEN) idx = 0;
+  if (idx == p->trans_extract_idx)
+    return FALSE;                          // queue full
+
+  t->status = I2CTransPending;
+
+  // disable irq
+  int_disable();
+  p->trans[p->trans_insert_idx] = t;
+  p->trans_insert_idx = idx;
+  /* if peripheral is idle, start the transaction */
+  if (p->status == I2CIdle)
+    I2cSendStart(p);
+  /* else it will be started by the interrupt handler when the previous transactions completes */
+  // enable irq
+  int_enable();
+  return TRUE;
+}
+
+#endif /* USE_I2C2 */
+
+
diff --git a/sw/airborne/arm7/i2c_hw.h b/sw/airborne/arm7/i2c_hw.h
index ca73dd641e..bc4c53b9a5 100644
--- a/sw/airborne/arm7/i2c_hw.h
+++ b/sw/airborne/arm7/i2c_hw.h
@@ -199,28 +199,7 @@ extern void i2c1_hw_init(void);
 
 #ifdef USE_I2C2
 
-
-extern void i2c0_hw_init(void);
-
-#define I2c0SendAck()   { I2C0CONSET = _BV(AA); }
-#define I2c0Finished()  {                                               \
-    if (i2c0_finished) *i2c0_finished = TRUE;				\
-    i2c0_status = I2C_IDLE;						\
-}
-#define I2c0SendStop()  {						\
-    I2C0CONSET = _BV(STO);						\
-    I2c0Finished();                                                     \
-  }
-#define I2c0SendStart() { I2C0CONSET = _BV(STA); }
-#define I2c0SendByte(b) { I2C_DATA_REG = b; }
-
-#define I2c0Receive(_ack) {	    \
-    if (_ack) I2C0CONSET = _BV(AA); \
-    else I2C0CONCLR = _BV(AAC);	    \
-  }
-
-#define I2c0ClearStart() { I2C0CONCLR = _BV(STAC); }
-#define I2c0ClearIT() { I2C0CONCLR = _BV(SIC); }
+extern void i2c2_hw_init(void);
 
 #endif
 
diff --git a/sw/airborne/i2c.h b/sw/airborne/i2c.h
index f897b97856..2f28a04431 100644
--- a/sw/airborne/i2c.h
+++ b/sw/airborne/i2c.h
@@ -15,7 +15,8 @@ enum I2CTransactionStatus {
   I2CTransPending, 
   I2CTransRunning, 
   I2CTransSuccess, 
-  I2CTransFailed
+  I2CTransFailed,
+  I2CTransDone
 };
 
 enum I2CStatus { 
@@ -145,4 +146,41 @@ extern void i2c2_init(void);
 extern void   i2c_init(struct i2c_periph* p);
 extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t);
 
+#define I2CReceive(_p, _t, _s_addr, _len) { \
+  _t.type = I2CTransRx; \
+  _t.slave_addr = _s_addr; \
+  _t.len_r = _len; \
+  _t.len_w = 0; \
+  _t.stop_after_transmit = TRUE; \
+  i2c_submit(&(_p),&(_t)); \
+}
+
+#define I2CTransmit(_p, _t, _s_addr, _len) { \
+  _t.type = I2CTransTx; \
+  _t.slave_addr = _s_addr; \
+  _t.len_r = 0; \
+  _t.len_w = _len; \
+  _t.stop_after_transmit = TRUE; \
+  i2c_submit(&(_p),&(_t)); \
+}
+
+#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \
+  _t.type = I2CTransTx; \
+  _t.slave_addr = _s_addr; \
+  _t.len_r = 0; \
+  _t.len_w = _len; \
+  _t.stop_after_transmit = FALSE; \
+  i2c_submit(&(_p),&(_t)); \
+}
+
+#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \
+  _t.type = I2CTransTxRx; \
+  _t.slave_addr = _s_addr; \
+  _t.len_r = _len_r; \
+  _t.len_w = _len_w; \
+  _t.stop_after_transmit = TRUE; \
+  i2c_submit(&(_p),&(_t)); \
+}
+
+
 #endif /* I2C_H */
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 6fdd02a9b4..1c2e4d6c91 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -79,7 +79,7 @@
 #include "srf08.h"
 #endif
 
-#if defined USE_I2C0 || USE_I2C1
+#if defined USE_I2C0 || USE_I2C1 || USE_I2C2
 #include "i2c.h"
 #endif
 
@@ -741,6 +741,10 @@ void init_ap( void ) {
   i2c1_init();
 #endif
 
+#ifdef USE_I2C2
+  i2c2_init();
+#endif
+
 #ifdef USE_AIRSPEED_ETS
   airspeed_ets_init();
 #endif
diff --git a/sw/airborne/modules/sensors/infrared_i2c.c b/sw/airborne/modules/sensors/infrared_i2c.c
index e0e8bde59d..d3c4295a6a 100644
--- a/sw/airborne/modules/sensors/infrared_i2c.c
+++ b/sw/airborne/modules/sensors/infrared_i2c.c
@@ -21,7 +21,6 @@
  */
 
 #include "sensors/infrared_i2c.h"
-#include "i2c.h"
 #include "estimator.h"
 
 // IR I2C definitions
@@ -34,15 +33,6 @@
 #define IR_HOR_I2C_SELECT_IR1 (0 << 5)
 #define IR_HOR_I2C_SELECT_IR2 (1 << 5)
 
-#define IR_I2C_IDLE             0
-#define IR_I2C_READ_IR1         1
-#define IR_I2C_IR2_SELECTED     2
-#define IR_I2C_READ_TOP         3
-#define IR_I2C_READ_IR2         4
-#define IR_I2C_IR1_SELECTED     5
-#define IR_I2C_CONFIGURE_HOR    6
-#define IR_I2C_CONFIGURE_VER    7
-
 #ifndef IR_I2C_IR1_NEUTRAL
 #define IR_I2C_IR1_NEUTRAL 0
 #endif
@@ -138,28 +128,39 @@ int16_t ir_i2c_top;
 
 float ir_i2c_phi, ir_i2c_theta;
 
-volatile bool_t ir_i2c_done;
 bool_t ir_i2c_data_available;
 uint8_t ir_i2c_conf_word;
-bool_t ir_i2c_conf_done;
+bool_t ir_i2c_conf_hor_done, ir_i2c_conf_ver_done;
 
 // Local variables
-static uint8_t ir_i2c_status;
+#define IR_I2C_IDLE             0
+#define IR_I2C_READ_IR1         1
+#define IR_I2C_IR2_SELECTED     2
+#define IR_I2C_READ_IR2         3
+#define IR_I2C_IR1_SELECTED     4
+#define IR_I2C_CONFIGURE_HOR    5
+
+static uint8_t ir_i2c_hor_status;
 
 #define NO_CONF_WORD 0xff
 #define ValidConfWord(_x) (_x < 0x4)
 
+// I2C structure
+struct i2c_transaction irh_trans, irv_trans;
+
 //FIXME standard infrared should not ba ADC-dependent
 void ir_init(void) {}
 
 /** Initialisation
  */
 void infrared_i2c_init( void ) {
-  ir_i2c_done = TRUE;
   ir_i2c_data_available = FALSE;
-  ir_i2c_status = IR_I2C_IDLE;
+  ir_i2c_hor_status = IR_I2C_IDLE;
   ir_i2c_conf_word = IR_I2C_DEFAULT_CONF;
-  ir_i2c_conf_done = FALSE;
+  ir_i2c_conf_hor_done = FALSE;
+  ir_i2c_conf_ver_done = FALSE;
+  irh_trans.status = I2CTransDone;
+  irv_trans.status = I2CTransDone;
 
   // Initialisation of standard infrared interface
   ir_roll_neutral  = RadOfDeg(IR_ROLL_NEUTRAL_DEFAULT);
@@ -175,21 +176,31 @@ void infrared_i2c_init( void ) {
   ir_vertical_correction = IR_VERTICAL_CORRECTION;
 }
 
+#include "led.h"
 void infrared_i2c_update( void ) {
 #if ! (defined SITL || defined HITL)
-  if (ir_i2c_done && ir_i2c_status == IR_I2C_IDLE) {
-    if (ValidConfWord(ir_i2c_conf_word) && !ir_i2c_conf_done) {
-      i2c0_buf[0] = 0;
-      i2c0_buf[0] = ir_i2c_conf_word | IR_HOR_OC_BIT | IR_START_CONV;
-      i2c0_transmit(IR_HOR_I2C_ADDR, 1, &ir_i2c_done);
-      ir_i2c_done = FALSE;
-      ir_i2c_status = IR_I2C_CONFIGURE_HOR;
+  // IR horizontal
+  if (irh_trans.status == I2CTransDone && ir_i2c_hor_status == IR_I2C_IDLE) {
+    if (ValidConfWord(ir_i2c_conf_word) && !ir_i2c_conf_hor_done) {
+      irh_trans.buf[0] = ir_i2c_conf_word | IR_HOR_OC_BIT | IR_START_CONV ;
+      I2CTransmit(i2c2, irh_trans, IR_HOR_I2C_ADDR, 1);
+      ir_i2c_hor_status = IR_I2C_CONFIGURE_HOR;
     } else {
       // Read next values
-      i2c0_receive(IR_HOR_I2C_ADDR, 3, &ir_i2c_done);
-      ir_i2c_done = FALSE;
+      I2CReceive(i2c2, irh_trans, IR_HOR_I2C_ADDR, 3);
+      ir_i2c_data_available = FALSE;
+      ir_i2c_hor_status = IR_I2C_READ_IR1;
+    }
+  }
+  // IR vertical
+  if (irv_trans.status == I2CTransDone) {
+    if (ValidConfWord(ir_i2c_conf_word) && !ir_i2c_conf_ver_done) {
+      irv_trans.buf[0] = ir_i2c_conf_word | IR_VER_OC_BIT;
+      I2CTransmit(i2c2, irv_trans, IR_VER_I2C_ADDR, 1);
+    } else {
+      // Read next values
+      I2CReceive(i2c2, irv_trans, IR_VER_I2C_ADDR, 2);
       ir_i2c_data_available = FALSE;
-      ir_i2c_status = IR_I2C_READ_IR1;
     }
   }
 #else /* SITL || HITL */
@@ -198,79 +209,74 @@ void infrared_i2c_update( void ) {
 #endif
 }
 
-void infrared_i2c_event( void ) {
+void infrared_i2c_hor_event( void ) {
 #if ! (defined SITL || defined HITL)
-  switch (ir_i2c_status) {
+  irh_trans.status = I2CTransDone;
+  switch (ir_i2c_hor_status) {
     case IR_I2C_IDLE :
       break;
     case IR_I2C_READ_IR1 :
-      if (bit_is_set(i2c0_buf[2],7)) {
-        i2c0_receive(IR_HOR_I2C_ADDR, 3, &ir_i2c_done);
-        ir_i2c_done = FALSE;
+      if (bit_is_set(irh_trans.buf[2],7)) {
+        I2CReceive(i2c2, irh_trans, IR_HOR_I2C_ADDR, 3);
         break;
       }
       // Read IR1 value
-      ir_i2c_ir1 = (i2c0_buf[0]<<8) | i2c0_buf[1];
+      ir_i2c_ir1 = (irh_trans.buf[0]<<8) | irh_trans.buf[1];
       // Select IR2 channel
-      i2c0_buf[0] = 0;
-      i2c0_buf[0] = IR_HOR_I2C_SELECT_IR2 | IR_HOR_OC_BIT | ir_i2c_conf_word | IR_START_CONV;
-      i2c0_transmit(IR_HOR_I2C_ADDR, 1, &ir_i2c_done);
-      ir_i2c_done = FALSE;
-      ir_i2c_status = IR_I2C_IR2_SELECTED;
+      irh_trans.buf[0] = IR_HOR_I2C_SELECT_IR2 | IR_HOR_OC_BIT | ir_i2c_conf_word | IR_START_CONV;
+      I2CTransmit(i2c2, irh_trans, IR_HOR_I2C_ADDR, 1);
+      ir_i2c_hor_status = IR_I2C_IR2_SELECTED;
       break;
     case IR_I2C_IR2_SELECTED :
-      // IR2 selected, asking for TOP value
-      i2c0_receive(IR_VER_I2C_ADDR, 2, &ir_i2c_done);
-      ir_i2c_done = FALSE;
-      ir_i2c_status = IR_I2C_READ_TOP;
-      break;
-    case IR_I2C_READ_TOP :
-      // Read TOP value
-      ir_i2c_top = (i2c0_buf[0]<<8) | i2c0_buf[1];
-      // Asking for IR2 value
-      i2c0_receive(IR_HOR_I2C_ADDR, 3, &ir_i2c_done);
-      ir_i2c_done = FALSE;
-      ir_i2c_status = IR_I2C_READ_IR2;
+      // IR2 selected, asking for IR2 value
+      I2CReceive(i2c2, irh_trans, IR_HOR_I2C_ADDR, 3);
+      ir_i2c_hor_status = IR_I2C_READ_IR2;
       break;
     case IR_I2C_READ_IR2 :
       // Read IR2 value
-      if (bit_is_set(i2c0_buf[2],7)) {
-        i2c0_receive(IR_HOR_I2C_ADDR, 3, &ir_i2c_done);
-        ir_i2c_done = FALSE;
+      if (bit_is_set(irh_trans.buf[2],7)) {
+        I2CReceive(i2c2, irh_trans, IR_HOR_I2C_ADDR, 3);
         break;
       }
-      ir_i2c_ir2 = (i2c0_buf[0]<<8) | i2c0_buf[1];
+      ir_i2c_ir2 = (irh_trans.buf[0]<<8) | irh_trans.buf[1];
+      // Update estimator
       ir_i2c_data_available = TRUE;
+      ir_update();
+      estimator_update_state_infrared();
       // Select IR1 channel
-      i2c0_buf[0] = 0;
-      i2c0_buf[0] = IR_HOR_I2C_SELECT_IR1 | IR_HOR_OC_BIT | ir_i2c_conf_word | IR_START_CONV;
-      i2c0_transmit(IR_HOR_I2C_ADDR, 1, &ir_i2c_done);
-      ir_i2c_done = FALSE;
-      ir_i2c_status = IR_I2C_IR1_SELECTED;
+      irh_trans.buf[0] = IR_HOR_I2C_SELECT_IR1 | IR_HOR_OC_BIT | ir_i2c_conf_word | IR_START_CONV;
+      I2CTransmit(i2c2, irh_trans, IR_HOR_I2C_ADDR, 1);
+      ir_i2c_hor_status = IR_I2C_IR1_SELECTED;
       break;
     case IR_I2C_IR1_SELECTED :
       // End reading cycle
-      ir_update();
-      estimator_update_state_infrared();
-      ir_i2c_status = IR_I2C_IDLE;
+      ir_i2c_hor_status = IR_I2C_IDLE;
       break;
     case IR_I2C_CONFIGURE_HOR :
-      // HOR configured, now configuring TOP
-      i2c0_buf[0] = 0;
-      i2c0_buf[0] = ir_i2c_conf_word | IR_VER_OC_BIT;
-      i2c0_transmit(IR_VER_I2C_ADDR, 1, &ir_i2c_done);
-      ir_i2c_done = FALSE;
-      ir_i2c_status = IR_I2C_CONFIGURE_VER;
-      break;
-    case IR_I2C_CONFIGURE_VER :
-      // VER configured, end conf cycle
-      ir_i2c_conf_done = TRUE;
-      ir_i2c_status = IR_I2C_IDLE;
+      // End conf cycle
+      ir_i2c_conf_hor_done = TRUE;
+      ir_i2c_hor_status = IR_I2C_IDLE;
       break;
   }
 #endif /* !SITL && !HITL */
 }
 
+void infrared_i2c_ver_event( void ) {
+#if ! (defined SITL || defined HITL)
+  irv_trans.status = I2CTransDone;
+  // Read TOP value
+  if (irv_trans.type == I2CTransRx) {
+    ir_i2c_top = (irv_trans.buf[0]<<8) | irv_trans.buf[1];
+    ir_i2c_data_available = TRUE;
+    ir_update();
+    estimator_update_state_infrared();
+  }
+  if (irv_trans.type == I2CTransTx) {
+    ir_i2c_conf_ver_done = TRUE;
+  }
+#endif /* !SITL && !HITL */
+}
+
 #include "stdio.h"
 
 void ir_update(void) {
diff --git a/sw/airborne/modules/sensors/infrared_i2c.h b/sw/airborne/modules/sensors/infrared_i2c.h
index 76431a1fd5..034ac5752a 100644
--- a/sw/airborne/modules/sensors/infrared_i2c.h
+++ b/sw/airborne/modules/sensors/infrared_i2c.h
@@ -30,6 +30,7 @@
 #include "std.h"
 #include "airframe.h"
 #include "infrared.h"
+#include "i2c.h"
 
 extern int16_t ir_i2c_ir1;
 extern int16_t ir_i2c_ir2;
@@ -37,13 +38,26 @@ extern int16_t ir_i2c_top;
 extern volatile bool_t ir_i2c_done;
 extern bool_t ir_i2c_data_available;
 extern uint8_t ir_i2c_conf_word;
+extern bool_t ir_i2c_conf_hor_done, ir_i2c_conf_ver_done;
+
+extern struct i2c_transaction irh_trans, irv_trans;
 
 extern void infrared_i2c_init( void );
 extern void infrared_i2c_update( void );
-extern void infrared_i2c_event( void );
+extern void infrared_i2c_hor_event( void );
+extern void infrared_i2c_ver_event( void );
 
-#define infrared_i2cEvent() { if (ir_i2c_done) infrared_i2c_event();	}
+#define infrared_i2cEvent() { \
+  if (irh_trans.status == I2CTransSuccess) infrared_i2c_hor_event(); \
+  if (irv_trans.status == I2CTransSuccess) infrared_i2c_ver_event(); \
+}
 
 #define infrared_i2cDownlink() DOWNLINK_SEND_DEBUG_IR_I2C(DefaultChannel, &ir_i2c_ir1, &ir_i2c_ir2, &ir_i2c_top)
 
+#define infrared_i2c_SetConfWord(_v) { \
+  ir_i2c_conf_hor_done = FALSE; \
+  ir_i2c_conf_ver_done = FALSE; \
+  ir_i2c_conf_word = _v; \
+}
+
 #endif // INFRARED_I2C_H