diff --git a/sw/airborne/arm7/include/LPC21xx.h b/sw/airborne/arm7/include/LPC21xx.h
index cfdc674d01..b8da419951 100644
--- a/sw/airborne/arm7/include/LPC21xx.h
+++ b/sw/airborne/arm7/include/LPC21xx.h
@@ -146,16 +146,16 @@
 
 ///////////////////////////////////////////////////////////////////////////////
 // I2C Interface
-#define I2C             ((i2cRegs_t *)0xE001C000)
+#define I2C0             ((i2cRegs_t *)0xE001C000)
 
 // I2C Registers
-#define I2CONSET        I2C->conset     /* Control Set Register */
-#define I2STAT          I2C->stat       /* Status Register */
-#define I2DAT           I2C->dat        /* Data Register */
-#define I2ADR           I2C->adr        /* Slave Address Register */
-#define I2SCLH          I2C->sclh       /* SCL Duty Cycle Register (high half word) */
-#define I2SCLL          I2C->scll       /* SCL Duty Cycle Register (low half word) */
-#define I2CONCLR        I2C->conclr     /* Control Clear Register */
+#define I2C0CONSET        I2C0->conset     /* Control Set Register */
+#define I2C0STAT          I2C0->stat       /* Status Register */
+#define I2C0DAT           I2C0->dat        /* Data Register */
+#define I2C0ADR           I2C0->adr        /* Slave Address Register */
+#define I2C0SCLH          I2C0->sclh       /* SCL Duty Cycle Register (high half word) */
+#define I2C0SCLL          I2C0->scll       /* SCL Duty Cycle Register (low half word) */
+#define I2C0CONCLR        I2C0->conclr     /* Control Clear Register */
 
 ///////////////////////////////////////////////////////////////////////////////
 // Serial Peripheral Interface 0 (SPI0)
diff --git a/sw/airborne/arm7/main_test_i2c.c b/sw/airborne/arm7/main_test_i2c.c
index 7375b50455..cf4ab2fda6 100644
--- a/sw/airborne/arm7/main_test_i2c.c
+++ b/sw/airborne/arm7/main_test_i2c.c
@@ -18,6 +18,8 @@ static inline void main_dl_parse_msg( void );
 
 int main( void ) {
   main_init();
+  /* send start_bit */
+  I2C0CONSET = 0x60;
   while (1) {
     if (sys_time_periodic())
       main_periodic_task();
@@ -78,4 +80,70 @@ static inline void main_dl_parse_msg(void) {
 }
 
 
+/* SDA0 on P0.3 */
+/* SCL0 on P0.2 */
 
+/* A0 A1 A2 are low */
+#define SLAVE_ADDR 0x20
+
+void i2c0_ISR(void) __attribute__((naked));
+
+static inline void main_i2c_init ( void ) {
+  /* set P0.2 and P0.3 to I2C0 */
+  PINSEL0 |= 1 << 4 | 1 << 6;
+  /* clear all flags */
+  I2C0CONCLR = 0x6C;
+  /* enable I2C */
+  I2C0CONSET = 0x40;
+  /* set bitrate */
+  I2C0SCLL = 200;  
+  I2C0SCLH = 200;  
+  
+  //  I2C0CONSET = ;
+  // initialize the interrupt vector
+  VICIntSelect &= ~VIC_BIT(VIC_I2C0);  // I2C0 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_I2C0);    // I2C0 interrupt enabled
+  VICVectCntl9 = VIC_ENABLE | VIC_I2C0;
+  VICVectAddr9 = (uint32_t)i2c0_ISR;    // address of the ISR
+  
+}
+
+void i2c0_ISR(void)
+{
+  // perform proper ISR entry so thumb-interwork works properly
+  ISR_ENTRY();
+  uint32_t state = I2C0STAT;
+
+  switch (state) {
+  case 8:
+    /* start condition transmitted */
+    /* send slave addr + W */
+    I2C0DAT = 0x74;
+    /* clear SI and start flag */
+    I2C0CONCLR = 0x28;
+    
+    break;
+    
+  case 24:
+    /* ack received from slave for address */
+    /* send data */
+    I2C0DAT = 0x55;
+    /* clear SI */
+    I2C0CONCLR = 0x8;
+  break;
+  
+  /* ack received from slave for byte */
+  case 40:
+    /* transmitt stop condition */
+    I2C0CONSET = 0x10;
+    /* clear SI */
+    I2C0CONCLR = 0x8;
+    break;
+
+  default:
+    break;
+  }
+
+  VICVectAddr = 0x00000000;             // clear this interrupt from the VIC
+  ISR_EXIT();                           // recover registers and return
+}