From fc0a95f4ae681ec5aa060e078e1d9f0637a00dec Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 7 Jul 2011 09:03:46 +0200
Subject: [PATCH 01/74] Revert the standalone HMC5843 module: you can now use
 the full imu module aspirin/ppzuavimu

---
 conf/modules/mag_hmc5843.xml              |  19 ----
 sw/airborne/modules/sensors/mag_hmc5843.c |  55 ----------
 sw/airborne/modules/sensors/mag_hmc5843.h |  33 ------
 sw/airborne/peripherals/hmc5843.c         | 121 +++++++---------------
 sw/airborne/peripherals/hmc5843.h         |   6 +-
 5 files changed, 39 insertions(+), 195 deletions(-)
 delete mode 100644 conf/modules/mag_hmc5843.xml
 delete mode 100644 sw/airborne/modules/sensors/mag_hmc5843.c
 delete mode 100644 sw/airborne/modules/sensors/mag_hmc5843.h

diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml
deleted file mode 100644
index c8439d8e3e..0000000000
--- a/conf/modules/mag_hmc5843.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-<module name="sensors">
-  <!-- <depend conflict="ins" -->
-  <header>
-    <file name="mag_hmc5843.h"/>
-  </header>
-  <init fun="hmc5843_module_init()"/>
-  <periodic fun="hmc5843_module_periodic()" freq="60"/>
-  <event fun="hmc5843_module_event()"/>
-  <makefile>
-    <define name="USE_I2C" />
-    <file name="mag_hmc5843.c"/>
-    <file name="../../peripherals/hmc5843.c"/>
-    <define name="HMC5843_I2C_DEVICE" value="i2c0" />
-    <define name="USE_I2C0" />
-    <define name="HMC5843_NO_IRQ" />
-  </makefile>
-</module>
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
deleted file mode 100644
index cc3965c14d..0000000000
--- a/sw/airborne/modules/sensors/mag_hmc5843.c
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * 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.
- *
- */
-#include "estimator.h"
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "downlink.h"
-#include <math.h>
-
-#include "../../peripherals/hmc5843.h"
-
-
-int32_t mag_x, mag_y, mag_z;
-bool_t mag_valid;
-
-
-#ifndef DOWNLINK_DEVICE
-#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
-#endif
-
-
-void hmc5843_module_init( void ) {
-  hmc5843_init();
-}
-
-void hmc5843_module_periodic ( void )
-{
-  hmc5843_periodic();
-  mag_x = hmc5843.data.value[0];
-  mag_y = hmc5843.data.value[1];
-  mag_z = hmc5843.data.value[2];
-  RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
-}
-
-void hmc5843_module_event( void )
-{
-  hmc5843_idle_task();
-}
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h
deleted file mode 100644
index d21dbb5037..0000000000
--- a/sw/airborne/modules/sensors/mag_hmc5843.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * 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.
- *
- */
-
-#ifndef HMC5843__H
-#define HMC5843__H
-
-#include "std.h"
-#include "mcu_periph/i2c.h"
-
-extern int32_t mag_x, mag_y, mag_z;
-
-extern void hmc5843_module_init( void );
-extern void hmc5843_module_periodic( void );
-extern void hmc5843_module_event( void );
-
-#endif // HMC5843__H
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 4eac86d75a..f02c148312 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -1,6 +1,4 @@
 #include "peripherals/hmc5843.h"
-#include "mcu_periph/i2c.h"
-#include "led.h"
 
 #define HMC5843_TIMEOUT 100
 
@@ -9,18 +7,12 @@
 struct Hmc5843 hmc5843;
 void exti9_5_irq_handler(void);
 
-#ifndef HMC5843_I2C_DEVICE
-#define HMC5843_I2C_DEVICE i2c2
-#endif
-
 void hmc5843_init(void)
 {
-  hmc5843.i2c_trans.status = I2CTransSuccess;
-  hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+    hmc5843.i2c_trans.status = I2CTransSuccess;
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
 
-#ifndef HMC5843_NO_IRQ
-  hmc5843_arch_init();
-#endif
+    hmc5843_arch_init();
 }
 
 // blocking, only intended to be called for initialization
@@ -30,101 +22,62 @@ static void send_config(void)
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
   hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
   hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
+  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
   hmc5843.i2c_trans.buf[1] = 0x01<<5;
   hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
+  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
   hmc5843.i2c_trans.buf[1] = 0x00;
   hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
+  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending);
 
 }
 
-#ifdef HMC5843_NO_IRQ
-volatile uint8_t fake_mag_eoc = 0;
-static uint8_t mag_eoc(void)
-{
-  return fake_mag_eoc;
-}
-#endif
-
 void hmc5843_idle_task(void)
 {
-  if (hmc5843.i2c_trans.status == I2CTransFailed) {
-    hmc5843.sent_tx = 0;
-    hmc5843.sent_rx = 0;
-  }
-
-  if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return;
-
-  if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) {
-    if (HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)) {
-      hmc5843.i2c_trans.type = I2CTransTx;
-      hmc5843.i2c_trans.len_w = 1;
-      hmc5843.i2c_trans.buf[0] = 0x3;
-      i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
-      hmc5843.sent_tx = 1;
-      return;
+    if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
+      if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
+        hmc5843.ready_for_read = FALSE;
+        hmc5843.i2c_trans.type = I2CTransRx;
+        hmc5843.i2c_trans.len_r = 7;
+        i2c_submit(&i2c2, &hmc5843.i2c_trans);
+        hmc5843.reading = TRUE;
+      }
     }
-  }
 
-  if (hmc5843.sent_tx) {
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 6;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = 0x3;
-    i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
-    hmc5843.sent_rx = 1;
-    hmc5843.sent_tx = 0;
-    return;
-  }
-
-  if (hmc5843.sent_rx && hmc5843.i2c_trans.status == I2CTransSuccess) {
-    hmc5843.sent_rx = 0;
-    hmc5843.sent_tx = 0;
-    hmc5843.timeout = 0;
-    hmc5843.data_available = TRUE;
-    memcpy(hmc5843.data.buf, (const void*) hmc5843.i2c_trans.buf, 6);
-    for (int i = 0; i < 3; i++) {
-      hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
+    if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
+        hmc5843.timeout = 0;
+        hmc5843.data_available = TRUE;
+        hmc5843.reading = FALSE;
+        memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
+        for (int i = 0; i < 3; i++) {
+            hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
+        }
     }
-  }
 }
 
 void hmc5843_periodic(void)
 {
-  if (!hmc5843.initialized) {
-    send_config();
-    hmc5843.initialized = TRUE;
-  } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)){
+    if (!hmc5843.initialized) {
+        send_config();
+        hmc5843.initialized = TRUE;
+    } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
 #ifdef USE_HMC59843_ARCH_RESET
-    hmc5843_arch_reset();
+        hmc5843_arch_reset();
 #endif
-    hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = 0x3;
-    i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
-
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 6;
-    i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
-    hmc5843.timeout = 0;
-  }
-
-#ifdef HMC5843_NO_IRQ
-  // < 50Hz
-  fake_mag_eoc = 1;
-#endif
-
+        hmc5843.i2c_trans.type = I2CTransRx;
+        hmc5843.i2c_trans.len_r = 7;
+        i2c_submit(&i2c2, &hmc5843.i2c_trans);
+        hmc5843.reading = TRUE;
+        hmc5843.ready_for_read = FALSE;
+        hmc5843.timeout = 0;
+    }
 }
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index ab71e07d80..8f70f144b4 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -27,6 +27,8 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
+#include "peripherals/hmc5843_arch.h"
+
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
     uint32_t timeout;
@@ -42,12 +44,8 @@ struct Hmc5843 {
 
 extern struct Hmc5843 hmc5843;
 
-#ifndef HMC5843_NO_IRQ
-#include "peripherals/hmc5843_arch.h"
-
 extern void hmc5843_arch_init( void );
 extern void hmc5843_arch_reset( void );
-#endif
 
 extern void hmc5843_init(void);
 extern void hmc5843_periodic(void);

From e19148f406b9684c3de3db4a77dd20326293dc03 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 7 Jul 2011 15:37:22 +0200
Subject: [PATCH 02/74] HMC driver with continuous messages (100% load on I2C
 bus for debugging)

---
 sw/airborne/peripherals/hmc5843.c | 83 +++++++++++++++++--------------
 sw/airborne/peripherals/hmc5843.h |  8 ++-
 2 files changed, 51 insertions(+), 40 deletions(-)

diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index f02c148312..a0abba6f32 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -1,6 +1,6 @@
 #include "peripherals/hmc5843.h"
 
-#define HMC5843_TIMEOUT 100
+#define HMC5843_TIMEOUT 10
 
 #define bswap_16(x)   ((((x) & 0xFF00) >> 8) | (((x) & 0x00FF) << 8))
 
@@ -9,10 +9,10 @@ void exti9_5_irq_handler(void);
 
 void hmc5843_init(void)
 {
-    hmc5843.i2c_trans.status = I2CTransSuccess;
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+  hmc5843.i2c_trans.status = I2CTransSuccess;
+  hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
 
-    hmc5843_arch_init();
+  hmc5843_arch_init();
 }
 
 // blocking, only intended to be called for initialization
@@ -23,61 +23,68 @@ static void send_config(void)
   hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
   hmc5843.i2c_trans.buf[1] = 0x01<<5;
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
   hmc5843.i2c_trans.buf[1] = 0x00;
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
+
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 6;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
 }
 
 void hmc5843_idle_task(void)
 {
-    if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
-      if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
-        hmc5843.ready_for_read = FALSE;
-        hmc5843.i2c_trans.type = I2CTransRx;
-        hmc5843.i2c_trans.len_r = 7;
-        i2c_submit(&i2c2, &hmc5843.i2c_trans);
-        hmc5843.reading = TRUE;
-      }
-    }
 
-    if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
-        hmc5843.timeout = 0;
-        hmc5843.data_available = TRUE;
-        hmc5843.reading = FALSE;
-        memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
-        for (int i = 0; i < 3; i++) {
-            hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
-        }
+  // Wait for I2C transaction object to be released by the I2C driver before changing anything
+  if ((hmc5843.i2c_trans.status == I2CTransFailed) || (hmc5843.i2c_trans.status == I2CTransSuccess))
+{
+/*
+  // If transaction succeeded
+  if (hmc5843.i2c_trans.status == I2CTransSuccess) 
+  {
+    memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
+    for (int i = 0; i < 3; i++) {
+      hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
     }
+    hmc5843.data_available = TRUE;
+  }
+
+  // Start a new one
+  if (hmc5843.timeout > HMC5843_TIMEOUT)
+  {
+    hmc5843.timeout = 0;
+*/
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 6;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+  }
+
 }
 
 void hmc5843_periodic(void)
 {
-    if (!hmc5843.initialized) {
-        send_config();
-        hmc5843.initialized = TRUE;
-    } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
-#ifdef USE_HMC59843_ARCH_RESET
-        hmc5843_arch_reset();
-#endif
-        hmc5843.i2c_trans.type = I2CTransRx;
-        hmc5843.i2c_trans.len_r = 7;
-        i2c_submit(&i2c2, &hmc5843.i2c_trans);
-        hmc5843.reading = TRUE;
-        hmc5843.ready_for_read = FALSE;
-        hmc5843.timeout = 0;
-    }
+  if (!hmc5843.initialized) {
+    send_config();
+    hmc5843.initialized = TRUE;
+    // hmc5843.i2c_trans.status = I2CTransSuccess;
+  }
+
+  hmc5843.timeout++;
 }
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index 8f70f144b4..06b9ff1dcf 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -27,8 +27,6 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
-#include "peripherals/hmc5843_arch.h"
-
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
     uint32_t timeout;
@@ -44,8 +42,14 @@ struct Hmc5843 {
 
 extern struct Hmc5843 hmc5843;
 
+#define HMC5843_USE_INT
+
+#ifndef HMC5843_NO_IRQ
+#include "peripherals/hmc5843_arch.h"
+
 extern void hmc5843_arch_init( void );
 extern void hmc5843_arch_reset( void );
+#endif
 
 extern void hmc5843_init(void);
 extern void hmc5843_periodic(void);

From 63df177d6c8eca26d95a5c988a33f711fe785c3e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 7 Jul 2011 17:25:16 +0200
Subject: [PATCH 03/74] Analyse all I2C interrupts in detail...

---
 conf/autopilot/rotorcraft.makefile            |   2 +-
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 668 ++++++++++++++++++
 2 files changed, 669 insertions(+), 1 deletion(-)
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c

diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
index c2e3c421c2..47b3922eb2 100644
--- a/conf/autopilot/rotorcraft.makefile
+++ b/conf/autopilot/rotorcraft.makefile
@@ -102,7 +102,7 @@ ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 # I2C is needed for speed controllers and barometers on lisa
 ap.srcs += mcu_periph/i2c.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
+ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.cdw.c
 
 ap.srcs += $(SRC_FIRMWARE)/commands.c
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
new file mode 100644
index 0000000000..609716d845
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -0,0 +1,668 @@
+#include "mcu_periph/i2c.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+
+#define LEDSTART_ON() {}
+#define LEDSTART_OFF() {}
+
+static inline void LED1_ON()
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
+}
+
+static inline void LED1_OFF()
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
+}
+
+static inline void LED2_ON()
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
+}
+
+static inline void LED2_OFF()
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET );
+}
+
+static inline void LED_INIT(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  LED1_OFF();
+  LED2_OFF();
+}
+
+
+static void start_transaction(struct i2c_periph* p);
+static inline void end_of_transaction(struct i2c_periph *p);
+static inline void i2c_hard_reset(struct i2c_periph *p);
+static inline void i2c_reset_init(struct i2c_periph *p);
+
+#define I2C_BUSY 0x20
+
+#ifdef DEBUG_I2C
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); }
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); }
+#else
+//#define SPURIOUS_INTERRUPT(_periph, _status, _event) { periph->errors->unexpected_event_cnt++; abort_and_reset(_periph);}
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);}
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);}
+#endif
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef  I2C1_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 200000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef  I2C2_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 390000
+};
+#endif
+
+static inline void i2c_delay(void)
+{
+    for (__IO int j = 0; j < 50; j++);
+}
+
+static inline void i2c_apply_config(struct i2c_periph *p)
+{
+    I2C_Init(p->reg_addr, p->init_struct);
+}
+
+static inline void end_of_transaction(struct i2c_periph *p)
+{
+    p->trans_extract_idx++;
+    if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+      p->trans_extract_idx = 0;
+    /* if we have no more transaction to process, stop here */
+    if (p->trans_extract_idx == p->trans_insert_idx)
+      p->status = I2CIdle;
+    /* if not, start next transaction */
+    else
+      start_transaction(p);
+}
+
+static inline void abort_and_reset(struct i2c_periph *p) {
+    struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+    trans->status = I2CTransFailed;
+//    I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
+    i2c_hard_reset(p);
+//    I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
+    end_of_transaction(p);
+}
+
+#ifdef USE_I2C2
+static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+
+/*
+ * Start Requested
+ *
+ */
+static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+  if (event & I2C_FLAG_SB) {
+    if(trans->type == I2CTransRx) {
+      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+      periph->status = I2CAddrRdSent;
+    }
+    else {
+      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
+      periph->status = I2CAddrWrSent;
+    }
+  }
+}
+
+/*
+ * Addr WR sent
+ *
+ */
+
+static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+//    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+  if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
+    I2C_SendData(periph->reg_addr, trans->buf[0]);
+//    I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+    if (trans->len_w > 1) {
+      I2C_SendData(periph->reg_addr, trans->buf[1]);
+      periph->idx_buf = 2;
+      periph->status = I2CSendingByte;
+    }
+    else {
+      periph->idx_buf = 1;
+      if (trans->type == I2CTransTx) {
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+	LEDSTART_OFF();
+        periph->status = I2CStopRequested;
+      }
+      else {
+        I2C_GenerateSTART(periph->reg_addr, ENABLE);
+        periph->status = I2CRestartRequested;
+      }
+    }
+  }
+}
+
+/*
+ * Sending Byte
+ *
+ */
+static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+  if (event & I2C_FLAG_BTF) {
+    if (periph->idx_buf < trans->len_w) {
+      I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
+      periph->idx_buf++;
+    }
+    else {
+//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+      if (trans->type == I2CTransTx) {
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+	LEDSTART_OFF();
+        periph->status = I2CStopRequested;
+      }
+      else {
+        I2C_GenerateSTART(periph->reg_addr, ENABLE);
+        periph->status = I2CRestartRequested;
+      }
+    }
+  }
+  else
+    SPURIOUS_INTERRUPT(periph, I2CSendingByte, event);
+}
+
+/*
+ * Stop Requested
+ *
+ */
+static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+  /* bummer.... */
+
+  if (event & I2C_FLAG_RXNE) {
+    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+    if (periph->idx_buf < trans->len_r) {
+      trans->buf[periph->idx_buf] = read_byte;
+    }
+  }
+  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT|I2C_IT_BUF, DISABLE);  // should only need to disable evt, buf already disabled
+  trans->status = I2CTransSuccess;
+  end_of_transaction(periph);
+}
+
+/*
+ * Addr RD sent
+ *
+ */
+
+static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+  if ((event & I2C_FLAG_ADDR) && !(event & I2C_FLAG_TRA)) {
+    periph->idx_buf = 0;
+    if(trans->len_r == 1) {                                         // If we're going to read only one byte
+      I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);             // make sure it's gonna be nacked
+      I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and followed by a stop
+	LEDSTART_OFF();
+      periph->status = I2CReadingLastByte;                           // and remember we did
+    }
+    else {
+      I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);               // if it's more than one byte, ack it
+//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+      periph->status = I2CReadingByte;                               // and remember we did
+    }
+  }
+}
+
+/*
+ * Reading byte
+ *
+ */
+
+static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+  if (event & I2C_FLAG_RXNE) {
+    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+    if (periph->idx_buf < trans->len_r) {
+      trans->buf[periph->idx_buf] = read_byte;
+      periph->idx_buf++;
+      if (periph->idx_buf >= trans->len_r-1) {                    // We're reading our last byte
+        I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);         // give them a nack once it's done
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and follow with a stop
+	LEDSTART_OFF();
+        periph->status = I2CReadingLastByte;                        // remember we already trigered the stop
+      }
+    }
+  }
+}
+
+/*
+ * Reading last byte
+ *
+ */
+
+static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) 
+{
+  if (event & I2C_FLAG_BTF) {
+    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+    trans->buf[periph->idx_buf] = read_byte;
+  }
+  else if (event & I2C_FLAG_RXNE) {       // should really be BTF ?
+    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+    trans->buf[periph->idx_buf] = read_byte;
+  }
+
+  on_status_stop_requested(periph, trans, event);
+}
+
+/*
+ * Restart requested
+ *
+ */
+
+static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+  if (event & I2C_FLAG_SB) {
+    I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+    periph->status = I2CAddrRdSent;
+  }
+}
+
+
+
+static inline void i2c_event(struct i2c_periph *p, uint32_t event)
+{
+  struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+  switch (p->status) {
+  case I2CStartRequested:
+    //LEDSTART_ON();
+    on_status_start_requested(p, trans, event);
+    break;
+  case I2CAddrWrSent:
+    on_status_addr_wr_sent(p, trans, event);
+    break;
+  case I2CSendingByte:
+    on_status_sending_byte(p, trans, event);
+    break;
+  case I2CStopRequested:
+    //LEDSTART_OFF();
+    on_status_stop_requested(p, trans, event);
+    break;
+  case I2CAddrRdSent:
+    on_status_addr_rd_sent(p, trans, event);
+    break;
+  case I2CReadingByte:
+    on_status_reading_byte(p, trans, event);
+    break;
+  case I2CReadingLastByte:
+    on_status_reading_last_byte(p, trans, event);
+    break;
+  case I2CRestartRequested:
+    on_status_restart_requested(p, trans, event);
+    break;
+  default:
+    OUT_OF_SYNC_STATE_MACHINE(p, p->status, event);
+    break;
+  }
+}
+
+static inline void i2c_error(struct i2c_periph *p)
+{
+  p->errors->er_irq_cnt;
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
+    p->errors->ack_fail_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_AF);
+  }
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
+    p->errors->miss_start_stop_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_BERR);
+  }
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
+    p->errors->arb_lost_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_ARLO);
+    //    I2C_AcknowledgeConfig(I2C2, DISABLE);
+    //    uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
+    //    I2C_GenerateSTOP(I2C2, ENABLE);
+  }
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
+    p->errors->over_under_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_OVR);
+  }
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
+    p->errors->pec_recep_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_PECERR);
+  }
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
+    p->errors->timeout_tlow_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_TIMEOUT);
+  }
+  if (I2C_GetITStatus(p->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+    p->errors->smbus_alert_cnt++;
+    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_SMBALERT);
+  }
+
+  abort_and_reset(p);
+}
+
+
+static inline void i2c_hard_reset(struct i2c_periph *p)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr;
+
+  I2C_DeInit(p->reg_addr);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
+  GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) {
+    // Raise SCL, wait until SCL is high (in case of clock stretching)
+    GPIO_SetBits(GPIOB, p->scl_pin);
+    while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+    i2c_delay();
+    
+    // Lower SCL, wait
+    GPIO_ResetBits(GPIOB, p->scl_pin);
+    i2c_delay();
+    
+    // Raise SCL, wait
+    GPIO_SetBits(GPIOB, p->scl_pin);
+    i2c_delay();
+  }
+  
+  // Generate a start condition followed by a stop condition
+  GPIO_SetBits(GPIOB, p->scl_pin);
+  i2c_delay();
+  GPIO_ResetBits(GPIOB, p->sda_pin);
+  i2c_delay();
+  GPIO_ResetBits(GPIOB, p->sda_pin);
+  i2c_delay();
+  
+  // Raise both SCL and SDA and wait for SCL high (in case of clock stretching)
+  GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+  while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+  
+  // Wait for SDA to be high
+  while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET);
+  
+  // SCL and SDA should be high at this point, bus should be free
+  // Return the GPIO pins to the alternate function
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+  
+  I2C_DeInit(p->reg_addr);
+  
+  i2c_apply_config(p);
+  
+  if (regs->SR2 & I2C_BUSY) {
+    // Reset the I2C block
+    I2C_SoftwareResetCmd(p->reg_addr, ENABLE);
+    I2C_SoftwareResetCmd(p->reg_addr, DISABLE);
+  }
+}
+
+static inline void i2c_reset_init(struct i2c_periph *p)
+{
+  // Reset bus and configure GPIO pins
+  i2c_hard_reset(p);
+
+  // enable peripheral
+  I2C_Cmd(p->reg_addr, ENABLE);
+
+  // enable error interrupts
+  I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE);
+}
+#endif /* USE_I2C2 */
+
+#ifdef USE_I2C1
+
+struct i2c_errors i2c1_errors;
+
+#include "my_debug_servo.h"
+
+void i2c1_hw_init(void) {
+
+  i2c1.reg_addr = I2C1;
+  i2c1.init_struct = &I2C1_InitStruct;
+  i2c1.scl_pin = GPIO_Pin_6;
+  i2c1.sda_pin = GPIO_Pin_7;
+  i2c1.errors = &i2c1_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c1_errors);
+
+}
+
+
+void i2c1_ev_irq_handler(void) {
+
+  uint32_t event = I2C_GetLastEvent(I2C1);
+  i2c_event(&i2c1, event);
+
+}
+
+
+void i2c1_er_irq_handler(void) {
+  i2c_error(&i2c1);
+}
+
+#endif /* USE_I2C1 */
+
+
+
+
+
+#ifdef USE_I2C2
+
+//  dec      hex
+//  196609   30001        BUSY  MSL |                 SB
+//  458882   70082    TRA BUSY  MSL | TXE       ADDR
+//  458884   70084    TRA BUSY  MSL | TXE  BTF
+//  196609   30001        BUSY  MSL |                 SB
+//  196610   30002        BUSY  MSL |           ADDR
+//
+
+
+struct i2c_errors i2c2_errors;
+
+#include "my_debug_servo.h"
+
+void i2c2_hw_init(void) {
+
+  i2c2.reg_addr = I2C2;
+  i2c2.init_struct = &I2C2_InitStruct;
+  i2c2.scl_pin = GPIO_Pin_10;
+  i2c2.sda_pin = GPIO_Pin_11;
+  i2c2.errors = &i2c2_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c2_errors);
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  I2C_DeInit(I2C2);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C2 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C2 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C2 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  /* Configure I2C1 pins: SCL and SDA ------------------------------------------*/
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_StructInit(&GPIO_InitStructure);
+  GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_10 | GPIO_Pin_11;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  // Reset and initialize I2C HW
+  i2c_reset_init(&i2c2);
+
+  LED_INIT();
+}
+
+
+
+
+void i2c2_ev_irq_handler(void) {
+  uint32_t event = I2C_GetLastEvent(I2C2);
+
+  //if (event & I2C_FLAG_SB)
+  {
+    LED1_ON();
+  }
+  if (event & I2C_FLAG_TXE)
+  {
+  //  LED1_ON();
+    LED2_ON();
+  }
+  //else
+  {
+    //LED2_ON();
+  }
+
+
+  i2c_event(&i2c2, event);
+
+  LED1_OFF();
+  LED2_OFF();
+}
+
+void i2c2_er_irq_handler(void) {
+  i2c_error(&i2c2);
+
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+
+}
+
+#endif /* USE_I2C2 */
+
+
+
+bool_t i2c_idle(struct i2c_periph* p)
+{
+  return !I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY);
+}
+
+bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
+
+  uint8_t temp;
+  temp = p->trans_insert_idx + 1;
+  if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
+  if (temp == p->trans_extract_idx)
+    return FALSE;                          // queue full
+
+  t->status = I2CTransPending;
+
+
+  __disable_irq();
+  /* put transacation in queue */
+  p->trans[p->trans_insert_idx] = t;
+  p->trans_insert_idx = temp;
+
+  /* if peripheral is idle, start the transaction */
+  if (p->status == I2CIdle)
+    start_transaction(p);
+  /* else it will be started by the interrupt handler when the previous transactions completes */
+  __enable_irq();
+
+  return TRUE;
+}
+
+
+static void start_transaction(struct i2c_periph* p) {
+  p->idx_buf = 0;
+  p->status = I2CStartRequested;
+  I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
+  I2C_GenerateSTART(p->reg_addr, ENABLE);
+  LEDSTART_ON();
+}

From 5fad4669862f9c4e819c9fe117e4a672ab98408f Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 7 Jul 2011 18:28:05 +0200
Subject: [PATCH 04/74] debugging airframe

---
 conf/airframes/CDW/debug_i2c.xml | 280 +++++++++++++++++++++++++++++++
 1 file changed, 280 insertions(+)
 create mode 100644 conf/airframes/CDW/debug_i2c.xml

diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
new file mode 100644
index 0000000000..5d75d389b1
--- /dev/null
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -0,0 +1,280 @@
+<!-- this is an asctec frame equiped with  Lisa/L#3 and asctec V2 controllers -->
+
+<airframe name="booz2_a7">
+
+  <servos min="0" neutral="0" max="0xff">
+    <servo name="FRONT"   no="0" min="0" neutral="0" max="255"/>
+    <servo name="BACK"    no="1" min="0" neutral="0" max="255"/>
+    <servo name="LEFT"    no="2" min="0" neutral="0" max="255"/>
+    <servo name="RIGHT"   no="3" min="0" neutral="0" max="255"/>
+  </servos>
+
+  <commands>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <!-- for the sim -->
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+  </section>
+
+
+  <section name="SUPERVISION" prefix="SUPERVISION_">
+    <define name="MIN_MOTOR" value="3"/>
+    <define name="MAX_MOTOR" value="200"/>
+    <define name="TRIM_A" value="0"/>
+    <define name="TRIM_E" value="0"/>
+    <define name="TRIM_R" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
+    <define name="ROLL_COEF"   value="{  0  ,    0,  256, -256 }"/>
+    <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
+    <define name="YAW_COEF"    value="{  256,  256, -256, -256 }"/>
+    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+
+    <define name="GYRO_P_NEUTRAL" value="32858"/>
+    <define name="GYRO_Q_NEUTRAL" value="33152"/>
+    <define name="GYRO_R_NEUTRAL" value="31779"/>
+    <define name="GYRO_P_SENS" value=" 1.101357422" integer="16"/>
+    <define name="GYRO_Q_SENS" value=" 1.122670898" integer="16"/>
+    <define name="GYRO_R_SENS" value=" 1.104890137" integer="16"/>
+
+
+    <define name="ACCEL_X_NEUTRAL" value="32899"/>
+    <define name="ACCEL_Y_NEUTRAL" value="33281"/>
+    <define name="ACCEL_Z_NEUTRAL" value="32569"/>
+    <define name="ACCEL_X_SENS" value="2.57702956051" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="2.57835230627" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="2.54311399868" integer="16"/>
+
+    <define name="MAG_X_NEUTRAL" value="58"/>
+    <define name="MAG_Y_NEUTRAL" value="92"/>
+    <define name="MAG_Z_NEUTRAL" value="-3"/>
+    <define name="MAG_X_SENS" value="4.70018395734" integer="16"/>
+    <define name="MAG_Y_SENS" value="4.884119848" integer="16"/>
+    <define name="MAG_Z_SENS" value="2.59926404993" integer="16"/>
+
+    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 135.)"/>
+
+  </section>
+
+ <section name="AUTOPILOT">
+   <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+   <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+   <define name="MODE_AUTO2"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+ </section>
+
+ <section name="BAT">
+   <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+   <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+   <define name="BATTERY_SENS"   value="0.48" integer="16"/>
+ </section>
+
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+
+    <define name="GAIN_P" value="-400"/>
+    <define name="GAIN_Q" value="-400"/>
+    <define name="GAIN_R" value="-350"/>
+
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="DEADBAND_R"     value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
+
+    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+   <!--  gaui props
+    <define name="PHI_PGAIN"  value="-900"/>
+    <define name="PHI_DGAIN"  value="-200"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-900"/>
+    <define name="THETA_DGAIN"  value="-200"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-900"/>
+    <define name="PSI_DGAIN"  value="-200"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <define name="PHI_DDGAIN"   value=" 200"/>
+    <define name="THETA_DDGAIN" value=" 200"/>
+    <define name="PSI_DDGAIN"   value=" 200"/>
+  -->
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-2000"/>
+    <define name="PHI_DGAIN"  value="-400"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-2000"/>
+    <define name="THETA_DGAIN"  value="-400"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-2000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
+
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="H_X" value=" 0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value=" 0.85490967783446"/>
+  </section>
+
+
+  <section name="INS" prefix="INS_">
+    <define name="BARO_SENS" value="10." integer="16"/>
+    <define name="UNTILT_ACCEL" value="1"/>
+  </section>
+
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+   <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+   <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+   <define name="MAX_SUM_ERR" value="2000000"/>
+
+<!--
+   <define name="HOVER_KP"    value="-500"/>
+   <define name="HOVER_KD"    value="-200"/>
+   <define name="HOVER_KI"    value="0"/>
+-->
+   <define name="HOVER_KP"     value="-150"/>
+   <define name="HOVER_KD"     value="-80"/>
+   <define name="HOVER_KI"     value="0"/>
+
+   <!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
+   <define name="RC_CLIMB_COEF" value ="163"/>
+   <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
+   <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
+    <!-- <define name="INV_M" value ="0.2"/> -->
+  </section>
+
+
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+   <define name="PGAIN" value="-50"/>
+   <define name="DGAIN" value="-100"/>
+   <define name="IGAIN" value="-15"/>
+   <define name="NGAIN" value="-0"/>
+ </section>
+
+ <section name="MISC">
+   <define name="FACE_REINJ_1"  value="1024"/>
+ </section>
+
+ <section name="SIMULATOR" prefix="NPS_">
+   <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
+   <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
+ </section>
+
+<!-- 
+ <modules main_freq="512">
+   <load name="vehicle_interface_overo_link.xml"/>
+ </modules>
+-->
+
+  <firmware name="rotorcraft">
+<!--     <define name="USE_PERSISTENT_SETTINGS"/> -->
+<!--     <define name="USE_INS_NAV_INIT"/> -->
+     <define name="USE_ADAPT_HOVER"/>
+     <define name="FAILSAFE_GROUND_DETECT"/>
+     <define name="USE_GPS_ACC4R"/>
+     <target name="ap" board="lisa_l_1.0">
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="actuators"     type="asctec"/>
+    </target>
+    <target name="sim" board="pc">
+      <subsystem name="fdm"           type="nps"/>
+      <subsystem name="radio_control" type="ppm"/>
+      <subsystem name="actuators"     type="mkk"/>
+    </target>
+    <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="imu"           type="b2_v1.2"/>
+    <subsystem name="gps"           type="ublox">
+      <configure name="GPS_BAUD"    value="B57600"/>
+    </subsystem>
+    <!--subsystem name="ahrs"          type="cmpl"/-->
+    <subsystem name="stabilization"   type="euler"/>
+    <subsystem name="ahrs"          type="ic"/>
+    <subsystem name="ins"           type="hff"/>
+  </firmware>
+
+  <firmware name="lisa_l_test_progs">
+    <target name="test_led"                 board="lisa_l_1.0"/>
+    <target name="test_uart"                board="lisa_l_1.0"/>
+    <target name="test_servos"              board="lisa_l_1.0"/>
+    <target name="test_telemetry"           board="lisa_l_1.0"/>
+    <target name="test_baro"                board="lisa_l_1.0"/>
+    <target name="test_imu_b2"              board="lisa_l_1.0"/>
+    <target name="test_imu_b2_2"            board="lisa_l_1.0"/>
+    <target name="test_imu_aspirin"         board="lisa_l_1.0"/>
+    <target name="test_rc_spektrum"         board="lisa_l_1.0"/>
+    <target name="test_rc_ppm"              board="lisa_l_1.0"/>
+    <target name="test_adc"                 board="lisa_l_1.0"/>
+    <target name="test_hmc5843"             board="lisa_l_1.0"/>
+    <target name="test_itg3200"             board="lisa_l_1.0"/>
+    <target name="test_adxl345"             board="lisa_l_1.0"/>
+    <target name="test_esc_mkk_simple"      board="lisa_l_1.0"/>
+    <target name="test_esc_asctecv1_simple" board="lisa_l_1.0"/>
+    <target name="test_actuators_mkk"       board="lisa_l_1.0"/>
+    <target name="test_actuators_asctecv1"  board="lisa_l_1.0"/>
+    <target name="tunnel_sw"                board="lisa_l_1.0"/>
+    <target name="tunnel_hw"                board="lisa_l_1.0"/>
+  </firmware>
+
+  <firmware name="lisa_passthrough">
+    <target name="overo_test_passthrough" board="lisa_l_1.0"  >
+      <param name="HOST"          value="A7"/>
+      <param name="USER"          value=""/>
+      <param name="TARGET_DIR"    value="~"/>
+      <param name="PERIODIC_FREQ" value="512"/>
+    </target>
+    <target name="stm_passthrough" board="lisa_l_1.0">
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="imu" type="b2_v1.1"/>
+    </target>
+ </firmware>
+
+
+</airframe>

From e67fe4f52d6a7fdc50dae5a5f6916e254c90a1c2 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 7 Jul 2011 23:37:00 +0200
Subject: [PATCH 05/74] Main ISR Structure

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 62 +++++++++++++++++--
 1 file changed, 58 insertions(+), 4 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index 609716d845..cd9603ee53 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -148,7 +148,7 @@ static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_
 //    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
   if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
     I2C_SendData(periph->reg_addr, trans->buf[0]);
-//    I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+    I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
     if (trans->len_w > 1) {
       I2C_SendData(periph->reg_addr, trans->buf[1]);
       periph->idx_buf = 2;
@@ -231,7 +231,7 @@ static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_
     }
     else {
       I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);               // if it's more than one byte, ack it
-//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
       periph->status = I2CReadingByte;                               // and remember we did
     }
   }
@@ -265,6 +265,7 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_
 
 static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) 
 {
+      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
   if (event & I2C_FLAG_BTF) {
     uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
     trans->buf[periph->idx_buf] = read_byte;
@@ -546,13 +547,66 @@ void i2c2_hw_init(void) {
 
 
 void i2c2_ev_irq_handler(void) {
+  /*	
+	There are 7 possible reasons to get here:
+
+	If IT_EV_FEN
+
+	1 SB
+	2 ADDR
+	(3 ADDR10)
+	(4 STOPF)
+	5 BTF
+
+	If IT_EV_FEN AND IT_EV_BUF
+
+	6 RxNE
+	7 TxE   
+   */
+
   uint32_t event = I2C_GetLastEvent(I2C2);
 
+  ////////////////////////////////////////
+  // Start Condition Met in Master Mode
+  if (event & I2C_FLAG_SB)
+  {
+    // Waiting for Start
+    // Waiting for Restart
+  }
+  // Master Sent a Slave Address
+  else if (event & I2C_FLAG_ADDR)
+  {
+    // Read
+    {
+      // One
+      // More
+    }
+    // Write
+    {
+      // Zero bytes
+      // One
+      // More
+    }
+  }
+  // Buffer Can accept the next byte for transmission
+  else if (event & I2C_FLAG_TxE)
+  {
+    // Not Last
+    // Last
+  }
+  // Receiver has new data (means: the reception of the next byte will be delayed until the data of the current transmission can be copied to the buffer)
+  else if (event & I2C_FLAG_BTF)
+  {
+    // Not Last
+    // Last
+  }
+  
+
   //if (event & I2C_FLAG_SB)
   {
     LED1_ON();
   }
-  if (event & I2C_FLAG_TXE)
+  if (event & I2C_FLAG_BTF)
   {
   //  LED1_ON();
     LED2_ON();
@@ -565,8 +619,8 @@ void i2c2_ev_irq_handler(void) {
 
   i2c_event(&i2c2, event);
 
-  LED1_OFF();
   LED2_OFF();
+  LED1_OFF();
 }
 
 void i2c2_er_irq_handler(void) {

From 068859eb12bcae8525adc033c3cf616660adaed7 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 00:17:51 +0200
Subject: [PATCH 06/74] BackBone

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 192 +++++++++++-------
 1 file changed, 123 insertions(+), 69 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index cd9603ee53..6679aa40f0 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -291,10 +291,133 @@ static inline void on_status_restart_requested(struct i2c_periph *periph, struct
 }
 
 
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+static inline void PPRZ_I2C_SEND_ADDR_READ(struct i2c_periph *periph, struct i2c_transaction* trans)
+{
+  I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+  periph->status = I2CAddrRdSent;
+}
+
+static inline void PPRZ_I2C_SEND_ADDR_WRITE(struct i2c_periph *periph, struct i2c_transaction* trans)
+{
+  I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
+  periph->status = I2CAddrWrSent;
+}
+
+static inline void PPRZ_I2C_RESTART(void)
+{
+  
+}
+
+static inline uint8_t PPRZ_I2C_READLAST(void)
+{
+  
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
 
 static inline void i2c_event(struct i2c_periph *p, uint32_t event)
 {
   struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+
+  /*	
+	There are 7 possible reasons to get here:
+
+	If IT_EV_FEN
+
+	1 SB
+	2 ADDR
+	(3 ADDR10)
+	(4 STOPF)
+	5 BTF
+
+	If IT_EV_FEN AND IT_EV_BUF
+
+	6 RxNE
+	7 TxE
+
+	-------------------------
+	We are always interested in all IT_EV_FEV: all are required.
+	We are only interested in IT_EV_BUF from 
+   */
+
+  ////////////////////////////////////////
+  // Start Condition Met in Master Mode
+  if (event & I2C_FLAG_SB)
+  {
+    // Waiting for Start
+    if (p->status == I2CStartRequested)
+    {
+      // Start Direct Read
+      if(trans->type == I2CTransRx)
+        PPRZ_I2C_SEND_ADDR_READ(p, trans);
+      // Start Writing
+      else
+        PPRZ_I2C_SEND_ADDR_WRITE(p, trans);
+    }
+    // Waiting for Restart: Always Read
+    else if (p->status == I2CStartRequested)
+    {
+      PPRZ_I2C_SEND_ADDR_READ(p, trans);
+    }
+    // Problem
+    else
+    {
+      PPRZ_I2C_STOP_AND_NEXT(p);
+    }
+  }
+  ////////////////////////////////////////
+  // Master Sent a Slave Address
+  else if (event & I2C_FLAG_ADDR)
+  {
+    // Read
+    if (p->status == I2CAddrRdSent)
+    {
+      // One
+      // More
+    }
+    // Write
+    else if (p->status == I2CAddrWrSent)
+    {
+      // Zero bytes
+      // One
+      // More
+    }
+  }
+  // Buffer Can accept the next byte for transmission
+  else if (event & I2C_FLAG_TxE)
+  {
+    // Not Last
+    // Last
+  }
+  // Receiver has new data (means: the reception of the next byte will be delayed until the data of the current transmission can be copied to the buffer)
+  else if (event & I2C_FLAG_BTF)
+  {
+    // Not Last
+    // Last
+  }
+  
+
+  //if (event & I2C_FLAG_SB)
+  {
+    LED1_ON();
+  }
+  if (event & I2C_FLAG_BTF)
+  {
+  //  LED1_ON();
+    LED2_ON();
+  }
+  //else
+  {
+    //LED2_ON();
+  }
+
+
+
   switch (p->status) {
   case I2CStartRequested:
     //LEDSTART_ON();
@@ -544,78 +667,9 @@ void i2c2_hw_init(void) {
 }
 
 
-
-
 void i2c2_ev_irq_handler(void) {
-  /*	
-	There are 7 possible reasons to get here:
-
-	If IT_EV_FEN
-
-	1 SB
-	2 ADDR
-	(3 ADDR10)
-	(4 STOPF)
-	5 BTF
-
-	If IT_EV_FEN AND IT_EV_BUF
-
-	6 RxNE
-	7 TxE   
-   */
-
   uint32_t event = I2C_GetLastEvent(I2C2);
 
-  ////////////////////////////////////////
-  // Start Condition Met in Master Mode
-  if (event & I2C_FLAG_SB)
-  {
-    // Waiting for Start
-    // Waiting for Restart
-  }
-  // Master Sent a Slave Address
-  else if (event & I2C_FLAG_ADDR)
-  {
-    // Read
-    {
-      // One
-      // More
-    }
-    // Write
-    {
-      // Zero bytes
-      // One
-      // More
-    }
-  }
-  // Buffer Can accept the next byte for transmission
-  else if (event & I2C_FLAG_TxE)
-  {
-    // Not Last
-    // Last
-  }
-  // Receiver has new data (means: the reception of the next byte will be delayed until the data of the current transmission can be copied to the buffer)
-  else if (event & I2C_FLAG_BTF)
-  {
-    // Not Last
-    // Last
-  }
-  
-
-  //if (event & I2C_FLAG_SB)
-  {
-    LED1_ON();
-  }
-  if (event & I2C_FLAG_BTF)
-  {
-  //  LED1_ON();
-    LED2_ON();
-  }
-  //else
-  {
-    //LED2_ON();
-  }
-
 
   i2c_event(&i2c2, event);
 

From 3b527aaedb5bc9493290786860e1e3d541aa2b13 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 00:51:33 +0200
Subject: [PATCH 07/74] Add some info

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 55 +++++++++++--------
 1 file changed, 33 insertions(+), 22 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index 6679aa40f0..3f5fbb3691 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -329,37 +329,52 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
 
 	If IT_EV_FEN
 
-	1 SB
-	2 ADDR
-	(3 ADDR10)
-	(4 STOPF)
-	5 BTF
+	1) SB		// Start Condition Success in Master mode
+	2) ADDR		// Address sent received Acknoledge
+	[3 ADDR10]	// -- 10bit address stuff
+	[4 STOPF]	// -- only for slaves: master has no stop interrupt
+	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
+
+	// Beware: using the buffered I2C has some interesting properties:
+	  -when receiving BTF only occurs after the 2nd received byte: after the first byte is received it is 
+           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost)
+	  -when transmitting, and writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C serializer and the buffer is ready for more. You can already
+	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+	    
+	// Beware: the order in which Status is read determines how flags are cleared.
 
 	If IT_EV_FEN AND IT_EV_BUF
 
-	6 RxNE
-	7 TxE
+	6) RxNE
+	7) TxE
 
 	-------------------------
 	We are always interested in all IT_EV_FEV: all are required.
-	We are only interested in IT_EV_BUF from 
+	We are only interested in buffer interrupts IT_EV_BUF when more data is comming: until (last-1)
    */
 
+
+  // This driver uses the hardware flags to keep track of its state
+  // 
+
+
   ////////////////////////////////////////
   // Start Condition Met in Master Mode
   if (event & I2C_FLAG_SB)
   {
-    // Waiting for Start
+    // Periph was waiting for Start
     if (p->status == I2CStartRequested)
     {
-      // Start Direct Read
+      // Send Read Slave Address
       if(trans->type == I2CTransRx)
         PPRZ_I2C_SEND_ADDR_READ(p, trans);
-      // Start Writing
+      // Send Write Slave Address
       else
         PPRZ_I2C_SEND_ADDR_WRITE(p, trans);
     }
-    // Waiting for Restart: Always Read
+    // Waiting for Restart: Always Rx
     else if (p->status == I2CStartRequested)
     {
       PPRZ_I2C_SEND_ADDR_READ(p, trans);
@@ -367,7 +382,7 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
     // Problem
     else
     {
-      PPRZ_I2C_STOP_AND_NEXT(p);
+      PPRZ_I2C_STOP_AND_NEXT(p, trans, I2CFailed);
     }
   }
   ////////////////////////////////////////
@@ -388,12 +403,15 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
       // More
     }
   }
+  ////////////////////////////////////////
   // Buffer Can accept the next byte for transmission
+  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts
   else if (event & I2C_FLAG_TxE)
   {
     // Not Last
     // Last
   }
+  ////////////////////////////////////////
   // Receiver has new data (means: the reception of the next byte will be delayed until the data of the current transmission can be copied to the buffer)
   else if (event & I2C_FLAG_BTF)
   {
@@ -402,19 +420,12 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
   }
   
 
-  //if (event & I2C_FLAG_SB)
-  {
-    LED1_ON();
-  }
+  LED1_ON();
+
   if (event & I2C_FLAG_BTF)
   {
-  //  LED1_ON();
     LED2_ON();
   }
-  //else
-  {
-    //LED2_ON();
-  }
 
 
 

From 18b6e0ef2889f3056155e428106dceecee77a132 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 09:47:26 +0200
Subject: [PATCH 08/74] Driver Conversion started (now broken)

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 152 ++++++++++--------
 1 file changed, 83 insertions(+), 69 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index 3f5fbb3691..a855abe20f 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -76,7 +76,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 390000
+      .I2C_ClockSpeed = 10000
 };
 #endif
 
@@ -148,7 +148,7 @@ static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_
 //    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
   if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
     I2C_SendData(periph->reg_addr, trans->buf[0]);
-    I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+//    I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
     if (trans->len_w > 1) {
       I2C_SendData(periph->reg_addr, trans->buf[1]);
       periph->idx_buf = 2;
@@ -231,7 +231,7 @@ static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_
     }
     else {
       I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);               // if it's more than one byte, ack it
-      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
       periph->status = I2CReadingByte;                               // and remember we did
     }
   }
@@ -265,7 +265,7 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_
 
 static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) 
 {
-      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
   if (event & I2C_FLAG_BTF) {
     uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
     trans->buf[periph->idx_buf] = read_byte;
@@ -295,21 +295,36 @@ static inline void on_status_restart_requested(struct i2c_periph *periph, struct
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
+static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* p)
+{
+  return I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY) == RESET;
+}
+
+static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* p) 
+{
+
+  I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
+
+  p->idx_buf = 0;
+  p->status = I2CStartRequested;
+  I2C_GenerateSTART(p->reg_addr, ENABLE);
+}
+
+static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
+{
+  p->idx_buf = 0;
+  p->status = I2CStartRequested;
+  I2C_GenerateSTART(p->reg_addr, ENABLE);
+}
+
 static inline void PPRZ_I2C_SEND_ADDR_READ(struct i2c_periph *periph, struct i2c_transaction* trans)
 {
   I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
-  periph->status = I2CAddrRdSent;
 }
 
 static inline void PPRZ_I2C_SEND_ADDR_WRITE(struct i2c_periph *periph, struct i2c_transaction* trans)
 {
   I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
-  periph->status = I2CAddrWrSent;
-}
-
-static inline void PPRZ_I2C_RESTART(void)
-{
-  
 }
 
 static inline uint8_t PPRZ_I2C_READLAST(void)
@@ -322,12 +337,16 @@ static inline uint8_t PPRZ_I2C_READLAST(void)
 
 static inline void i2c_event(struct i2c_periph *p, uint32_t event)
 {
+  // Check to make sure that user space has a message pending
   struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
 
   /*	
 	There are 7 possible reasons to get here:
 
 	If IT_EV_FEN
+	-------------------------
+
+	We are always interested in all IT_EV_FEV: all are required.
 
 	1) SB		// Start Condition Success in Master mode
 	2) ADDR		// Address sent received Acknoledge
@@ -346,22 +365,52 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
 	// Beware: the order in which Status is read determines how flags are cleared.
 
 	If IT_EV_FEN AND IT_EV_BUF
+	--------------------------
+
+	We are always interested in buffer interrupts IT_EV_BUF except in transmission when all data was sent
 
 	6) RxNE
 	7) TxE
 
-	-------------------------
-	We are always interested in all IT_EV_FEV: all are required.
-	We are only interested in buffer interrupts IT_EV_BUF when more data is comming: until (last-1)
+	--------------------------------------------------------------------------------------------------
+	// This driver uses only a subset of the pprz_i2c_states for several reasons:
+	// -we have less interrupts than the I2CStatus states (for efficiency)
+	// -status register flags better correspond to reality, especially in case of I2C errors
+
+	enum I2CStatus {
+	  I2CIdle,			// Dummy: Actual I2C Peripheral idle detection is safer with the 
+					// hardware status register flag I2C_FLAG_BUSY.
+
+	  I2CStartRequested,		// EV5: used to differentiate S en Sr
+	  I2CRestartRequested,		// EV5: used to differentiate S en Sr
+
+	  I2CSendingByte,		// Not used: using the hardware status reg I2C_FLAG_TRA
+	  I2CReadingByte,
+	  I2CAddrWrSent,		// Since we can do many things at once and we
+	  I2CAddrRdSent,		// have buffered sending, these states
+	  I2CSendingLastByte,		// do not correspond to the real state of the
+	  I2CReadingLastByte,		// STM I2C driver so they are not used
+	  I2CStopRequested,		
+
+	  I2CComplete,			// Used to provide the result
+	  I2CFailed
+	};
+
+	---------
+
+	The STM waits (holding SCL low) for user interaction:
+	a) after a master-start (waiting for address)
+	b) after an address (waiting for data)
+	   not during data sending when using buffered
+	c) after the last byte is transmitted (waiting for either stop or restart)
+	   not during data receiving when using buffered
+	   not after the last byte is received
+
    */
 
 
-  // This driver uses the hardware flags to keep track of its state
-  // 
-
-
-  ////////////////////////////////////////
-  // Start Condition Met in Master Mode
+  ////////////////////////////////////////////////////////
+  // Start Condition Met in Master Mode: STM Manual Ev5
   if (event & I2C_FLAG_SB)
   {
     // Periph was waiting for Start
@@ -386,7 +435,7 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
     }
   }
   ////////////////////////////////////////
-  // Master Sent a Slave Address
+  // Master Sent a Slave Address: STM Manual Ev6
   else if (event & I2C_FLAG_ADDR)
   {
     // Read
@@ -406,7 +455,7 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
   ////////////////////////////////////////
   // Buffer Can accept the next byte for transmission
   // --> this means we HAVE TO fill the buffer and/or disable buf interrupts
-  else if (event & I2C_FLAG_TxE)
+  else if (event & I2C_FLAG_TXE)
   {
     // Not Last
     // Last
@@ -418,48 +467,23 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
     // Not Last
     // Last
   }
+  ///////////////////////////////////////
+  else if (event & I2C_FLAG_BTF)
+  {
+    // Not Last
+    // Last
+  }
   
 
   LED1_ON();
 
-  if (event & I2C_FLAG_BTF)
+  if (event & I2C_FLAG_TRA)
   {
     LED2_ON();
   }
 
 
 
-  switch (p->status) {
-  case I2CStartRequested:
-    //LEDSTART_ON();
-    on_status_start_requested(p, trans, event);
-    break;
-  case I2CAddrWrSent:
-    on_status_addr_wr_sent(p, trans, event);
-    break;
-  case I2CSendingByte:
-    on_status_sending_byte(p, trans, event);
-    break;
-  case I2CStopRequested:
-    //LEDSTART_OFF();
-    on_status_stop_requested(p, trans, event);
-    break;
-  case I2CAddrRdSent:
-    on_status_addr_rd_sent(p, trans, event);
-    break;
-  case I2CReadingByte:
-    on_status_reading_byte(p, trans, event);
-    break;
-  case I2CReadingLastByte:
-    on_status_reading_last_byte(p, trans, event);
-    break;
-  case I2CRestartRequested:
-    on_status_restart_requested(p, trans, event);
-    break;
-  default:
-    OUT_OF_SYNC_STATE_MACHINE(p, p->status, event);
-    break;
-  }
 }
 
 static inline void i2c_error(struct i2c_periph *p)
@@ -745,12 +769,8 @@ void i2c2_er_irq_handler(void) {
 
 #endif /* USE_I2C2 */
 
-
-
-bool_t i2c_idle(struct i2c_periph* p)
-{
-  return !I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY);
-}
+/////////////////////////////
+// User-space Interaction
 
 bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
 
@@ -769,8 +789,8 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
   p->trans_insert_idx = temp;
 
   /* if peripheral is idle, start the transaction */
-  if (p->status == I2CIdle)
-    start_transaction(p);
+  if (PPRZ_I2C_IS_IDLE())
+    PPRZ_I2C_START_NEXT_TRANSACTION(p);
   /* else it will be started by the interrupt handler when the previous transactions completes */
   __enable_irq();
 
@@ -778,10 +798,4 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
 }
 
 
-static void start_transaction(struct i2c_periph* p) {
-  p->idx_buf = 0;
-  p->status = I2CStartRequested;
-  I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
-  I2C_GenerateSTART(p->reg_addr, ENABLE);
-  LEDSTART_ON();
-}
+

From 9c4273edba34350693838178b611febe338e42ad Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 10:23:12 +0200
Subject: [PATCH 09/74] Start Ready - Transmission ready

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 68 ++++++++++++-------
 1 file changed, 43 insertions(+), 25 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index a855abe20f..ced42b0462 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -265,7 +265,6 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_
 
 static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) 
 {
-//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
   if (event & I2C_FLAG_BTF) {
     uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
     trans->buf[periph->idx_buf] = read_byte;
@@ -302,18 +301,17 @@ static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* p)
 
 static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* p) 
 {
-
-  I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
+  p->status = I2CStartRequested;
 
   p->idx_buf = 0;
-  p->status = I2CStartRequested;
+  I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
   I2C_GenerateSTART(p->reg_addr, ENABLE);
 }
 
 static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 {
   p->idx_buf = 0;
-  p->status = I2CStartRequested;
+  p->status = I2CRestartRequested;
   I2C_GenerateSTART(p->reg_addr, ENABLE);
 }
 
@@ -410,7 +408,8 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
 
 
   ////////////////////////////////////////////////////////
-  // Start Condition Met in Master Mode: STM Manual Ev5
+  // START: Start Condition Met in Master Mode: 
+  // STM Manual Ev5
   if (event & I2C_FLAG_SB)
   {
     // Periph was waiting for Start
@@ -428,14 +427,48 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
     {
       PPRZ_I2C_SEND_ADDR_READ(p, trans);
     }
-    // Problem
+    // Problem: this problem need to be triggerd as if the 
+    // status was not OK then the buf size is also bad
     else
     {
       PPRZ_I2C_STOP_AND_NEXT(p, trans, I2CFailed);
     }
   }
+  ////////////////////////////////////////////////////////////////
+  // TRANSMIT: Buffer Can accept the next byte for transmission
+  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt 
+  //     will be triggered until a start/stop occurs which can be quite long = many spurrious interrupts)
+  // STM Manual Ev8
+  else if (event & I2C_FLAG_TXE) // only possible when TRA, not sending start/stop/addr
+  {
+    // Do we have more data? (yes! -> then neglect BTF: if it was set it just means we were too slow)
+    if (periph->idx_buf < trans->len_w) 
+    {
+      I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
+      periph->idx_buf++;
+      // Was this one the Last? -> Disable the buf interrupt (until next start) and wait for BTF
+      if (periph->idx_buf < trans->len_w)
+      {
+        I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+      } 
+    }
+    // STM Manual Ev8_2
+    else
+    {
+      // Ready -> Stop
+      if (trans->type == I2CTransTx) 
+      {
+        PPRZ_I2C_STOP_AND_NEXT(p, trans, I2CSuccess);
+      }
+      // Rx/Trans -> Restart
+      else 
+      {
+        PPRZ_I2C_RESTART(p);
+      }
+    }
+  }
   ////////////////////////////////////////
-  // Master Sent a Slave Address: STM Manual Ev6
+  // RECEIVE: either RXNE or ADDR
   else if (event & I2C_FLAG_ADDR)
   {
     // Read
@@ -452,23 +485,8 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
       // More
     }
   }
-  ////////////////////////////////////////
-  // Buffer Can accept the next byte for transmission
-  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts
-  else if (event & I2C_FLAG_TXE)
-  {
-    // Not Last
-    // Last
-  }
-  ////////////////////////////////////////
-  // Receiver has new data (means: the reception of the next byte will be delayed until the data of the current transmission can be copied to the buffer)
-  else if (event & I2C_FLAG_BTF)
-  {
-    // Not Last
-    // Last
-  }
-  ///////////////////////////////////////
-  else if (event & I2C_FLAG_BTF)
+  // 
+  else if (event & I2C_FLAG_RXNE)
   {
     // Not Last
     // Last

From 146f5279564d8a76181134065b8ab8d1aa544ca2 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 12:22:40 +0200
Subject: [PATCH 10/74] Receiveing Part

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 494 +++++++-----------
 1 file changed, 187 insertions(+), 307 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index ced42b0462..2f137bc5da 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -113,229 +113,89 @@ static inline void abort_and_reset(struct i2c_periph *p) {
 }
 
 #ifdef USE_I2C2
-static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
 
-/*
- * Start Requested
- *
- */
-static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-  if (event & I2C_FLAG_SB) {
-    if(trans->type == I2CTransRx) {
-      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
-      periph->status = I2CAddrRdSent;
-    }
-    else {
-      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
-      periph->status = I2CAddrWrSent;
-    }
-  }
-}
-
-/*
- * Addr WR sent
- *
- */
-
-static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-//    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-  if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
-    I2C_SendData(periph->reg_addr, trans->buf[0]);
-//    I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
-    if (trans->len_w > 1) {
-      I2C_SendData(periph->reg_addr, trans->buf[1]);
-      periph->idx_buf = 2;
-      periph->status = I2CSendingByte;
-    }
-    else {
-      periph->idx_buf = 1;
-      if (trans->type == I2CTransTx) {
-        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-	LEDSTART_OFF();
-        periph->status = I2CStopRequested;
-      }
-      else {
-        I2C_GenerateSTART(periph->reg_addr, ENABLE);
-        periph->status = I2CRestartRequested;
-      }
-    }
-  }
-}
-
-/*
- * Sending Byte
- *
- */
-static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-  if (event & I2C_FLAG_BTF) {
-    if (periph->idx_buf < trans->len_w) {
-      I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
-      periph->idx_buf++;
-    }
-    else {
-//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
-      if (trans->type == I2CTransTx) {
-        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-	LEDSTART_OFF();
-        periph->status = I2CStopRequested;
-      }
-      else {
-        I2C_GenerateSTART(periph->reg_addr, ENABLE);
-        periph->status = I2CRestartRequested;
-      }
-    }
-  }
-  else
-    SPURIOUS_INTERRUPT(periph, I2CSendingByte, event);
-}
-
-/*
- * Stop Requested
- *
- */
-static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-  /* bummer.... */
-
-  if (event & I2C_FLAG_RXNE) {
-    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-    if (periph->idx_buf < trans->len_r) {
-      trans->buf[periph->idx_buf] = read_byte;
-    }
-  }
-  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT|I2C_IT_BUF, DISABLE);  // should only need to disable evt, buf already disabled
-  trans->status = I2CTransSuccess;
-  end_of_transaction(periph);
-}
-
-/*
- * Addr RD sent
- *
- */
-
-static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-  if ((event & I2C_FLAG_ADDR) && !(event & I2C_FLAG_TRA)) {
-    periph->idx_buf = 0;
-    if(trans->len_r == 1) {                                         // If we're going to read only one byte
-      I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);             // make sure it's gonna be nacked
-      I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and followed by a stop
-	LEDSTART_OFF();
-      periph->status = I2CReadingLastByte;                           // and remember we did
-    }
-    else {
-      I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);               // if it's more than one byte, ack it
-//      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
-      periph->status = I2CReadingByte;                               // and remember we did
-    }
-  }
-}
-
-/*
- * Reading byte
- *
- */
-
-static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-  if (event & I2C_FLAG_RXNE) {
-    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-    if (periph->idx_buf < trans->len_r) {
-      trans->buf[periph->idx_buf] = read_byte;
-      periph->idx_buf++;
-      if (periph->idx_buf >= trans->len_r-1) {                    // We're reading our last byte
-        I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);         // give them a nack once it's done
-        I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and follow with a stop
-	LEDSTART_OFF();
-        periph->status = I2CReadingLastByte;                        // remember we already trigered the stop
-      }
-    }
-  }
-}
-
-/*
- * Reading last byte
- *
- */
-
-static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) 
-{
-  if (event & I2C_FLAG_BTF) {
-    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-    trans->buf[periph->idx_buf] = read_byte;
-  }
-  else if (event & I2C_FLAG_RXNE) {       // should really be BTF ?
-    uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-    trans->buf[periph->idx_buf] = read_byte;
-  }
-
-  on_status_stop_requested(periph, trans, event);
-}
-
-/*
- * Restart requested
- *
- */
-
-static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
-  if (event & I2C_FLAG_SB) {
-    I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
-    periph->status = I2CAddrRdSent;
-  }
-}
 
 
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
+// IDLE CHECK
+
 static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* p)
 {
   return I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY) == RESET;
 }
 
-static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* p) 
-{
-  p->status = I2CStartRequested;
+// (RE)START
 
+static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
+{
   p->idx_buf = 0;
   I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
   I2C_GenerateSTART(p->reg_addr, ENABLE);
 }
 
+static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* p) 
+{
+  /* if we have no more transaction to process, stop here */
+  if (p->trans_extract_idx == p->trans_insert_idx)
+  {
+    p->status = I2CIdle;
+  }
+  /* if not, start next transaction */
+  else
+  {
+    p->status = I2CStartRequested;
+    PPRZ_I2C_SEND_START(p);
+  }
+}
+
 static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 {
-  p->idx_buf = 0;
   p->status = I2CRestartRequested;
-  I2C_GenerateSTART(p->reg_addr, ENABLE);
+  PPRZ_I2C_SEND_START(p);
 }
 
-static inline void PPRZ_I2C_SEND_ADDR_READ(struct i2c_periph *periph, struct i2c_transaction* trans)
+static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_trans *trans, I2CStatus _status)
 {
-  I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
-}
+  // Finish Current
+  trans->status = _status;
+ 
+  // When finished successfully the I2C_FLAG_MLS will be cleared after the stop condition was issued.
+  // However: we do not need to wait for it to go the the next step, but if no stop condition was 
+  // sent than we are still talking to the same slave...
 
-static inline void PPRZ_I2C_SEND_ADDR_WRITE(struct i2c_periph *periph, struct i2c_transaction* trans)
-{
-  I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
-}
-
-static inline uint8_t PPRZ_I2C_READLAST(void)
-{
+  // todo: evaluate all possible routes to this function...
   
+  // a) stop condition was already sheduled?
+  // b) stop condition not yet sheduled?
+
+  // Jump to the next
+  periph->trans_extract_idx++;
+  if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+    periph->trans_extract_idx = 0;
+
+  PPRZ_I2C_START_NEXT_TRANSACTION(periph);
 }
 
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
-static inline void i2c_event(struct i2c_periph *p, uint32_t event)
+static inline void i2c_event(struct i2c_periph *p)
 {
-  // Check to make sure that user space has a message pending
+  // Referring to manual: 
+  // -Doc ID 13902 Rev 11
+
+
+  // Check to make sure that user space has an active transaction pending
+  if (p->trans_extract_idx == p->trans_insert_idx)
+  {
+    // no transaction?
+    p->errors->unexpected_event_cnt++;
+    return;
+  }
+
   struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
 
   /*	
@@ -407,8 +267,21 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
    */
 
 
-  ////////////////////////////////////////////////////////
-  // START: Start Condition Met in Master Mode: 
+  ///////////////////////////////////////////////////////////////////////////////////
+  // Reading the status:
+  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
+  //   the transmission of the byte is finished. At higher clock rates that can be
+  //   quite fast: so we allow no other interrupt to be triggered in between
+  //   reading the status and setting all needed flags
+
+  __disable_irq();
+  uint32_t event = I2C_GetLastEvent(periph->reg_addr);
+
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////
+  // START: Start Condition in Master Mode: 
   // STM Manual Ev5
   if (event & I2C_FLAG_SB)
   {
@@ -417,48 +290,54 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
     {
       // Send Read Slave Address
       if(trans->type == I2CTransRx)
-        PPRZ_I2C_SEND_ADDR_READ(p, trans);
+        I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
       // Send Write Slave Address
       else
-        PPRZ_I2C_SEND_ADDR_WRITE(p, trans);
+        I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
     }
     // Waiting for Restart: Always Rx
     else if (p->status == I2CStartRequested)
     {
-      PPRZ_I2C_SEND_ADDR_READ(p, trans);
+      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
     }
     // Problem: this problem need to be triggerd as if the 
     // status was not OK then the buf size is also bad
     else
     {
-      PPRZ_I2C_STOP_AND_NEXT(p, trans, I2CFailed);
+      PPRZ_I2C_HAS_FINISHED(p, trans, I2CFailed);
     }
   }
-  ////////////////////////////////////////////////////////////////
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////
   // TRANSMIT: Buffer Can accept the next byte for transmission
   // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt 
   //     will be triggered until a start/stop occurs which can be quite long = many spurrious interrupts)
   // STM Manual Ev8
-  else if (event & I2C_FLAG_TXE) // only possible when TRA, not sending start/stop/addr
+  else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and not sending start/stop/addr
   {
-    // Do we have more data? (yes! -> then neglect BTF: if it was set it just means we were too slow)
-    if (periph->idx_buf < trans->len_w) 
+    // Do we have more data? and there is buffer room? 
+    // (neglect BTF: if it was set it just means we were too slow)
+    while ((periph->idx_buf < trans->len_w) && (I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_TXE) == SET))
     {
       I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
       periph->idx_buf++;
       // Was this one the Last? -> Disable the buf interrupt (until next start) and wait for BTF
-      if (periph->idx_buf < trans->len_w)
+      // -we could gain a bit of efficiency by already starting the next action but for 
+      //  code-readability we will wait until the last tx-byte is sent
+      if (periph->idx_buf >= trans->len_w)
       {
         I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
       } 
     }
-    // STM Manual Ev8_2
-    else
+
+    // STM Manual Ev8_2 
+    if ((event & I2C_FLAG_BTF) && (periph->idx_buf >= trans->len_w))
     {
       // Ready -> Stop
       if (trans->type == I2CTransTx) 
       {
-        PPRZ_I2C_STOP_AND_NEXT(p, trans, I2CSuccess);
+        PPRZ_I2C_HAS_FINISHED(p, trans, I2CSuccess);
       }
       // Rx/Trans -> Restart
       else 
@@ -466,32 +345,53 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
         PPRZ_I2C_RESTART(p);
       }
     }
-  }
-  ////////////////////////////////////////
-  // RECEIVE: either RXNE or ADDR
-  else if (event & I2C_FLAG_ADDR)
-  {
-    // Read
-    if (p->status == I2CAddrRdSent)
+    // If we had no more data but got no BTF then there is a problem
+    else
     {
-      // One
-      // More
-    }
-    // Write
-    else if (p->status == I2CAddrWrSent)
-    {
-      // Zero bytes
-      // One
-      // More
+      PPRZ_I2C_HAS_FINISHED(p, trans, I2CFailed);
     }
   }
-  // 
-  else if (event & I2C_FLAG_RXNE)
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////
+  // RECEIVE:
+  // while receiving: the master needs to signal to the slave if more data is needed
+  else if ((event & I2C_FLAG_ADDR) || (event & I2C_FLAG_RXNE))
   {
-    // Not Last
-    // Last
+    // data is available every time RXNE is set. If BTF is set it means that 2 bytes are 
+    // ready to read (one in shift register) and I2C has stopped until the buffer can accept new data.
+    if (event & I2C_FLAG_RXNE) 
+    {
+      uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+      if (periph->idx_buf < trans->len_r) 
+      {
+        trans->buf[periph->idx_buf] = read_byte;
+        periph->idx_buf++;
+      }
+    }
+
+    // This last byte has arrived
+    if (periph->idx_buf >= trans->len_r) 
+    {
+      PPRZ_I2C_HAS_FINISHED(p, trans, I2CSuccess);
+    }
+    // Tell the Slave it will be the last one
+    else if (periph->idx_buf >= trans->len_r-1) 
+    {
+      I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);         // give them a nack once it's done
+      I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and follow with a stop
+    }
+    // Ask the Slave to send more
+    else
+    {
+      I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);
+    }
+
   }
-  
+
+  // Now re-enable IRQ... it's been too long  
+  __enable_irq();
+
 
   LED1_ON();
 
@@ -500,6 +400,8 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event)
     LED2_ON();
   }
 
+  LED2_OFF();
+  LED1_OFF();
 
 
 }
@@ -540,6 +442,58 @@ static inline void i2c_error(struct i2c_periph *p)
   }
 
   abort_and_reset(p);
+
+
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+    LED1_ON();
+    LED1_OFF();
+
 }
 
 
@@ -615,12 +569,13 @@ static inline void i2c_reset_init(struct i2c_periph *p)
 }
 #endif /* USE_I2C2 */
 
+
+
+
 #ifdef USE_I2C1
 
 struct i2c_errors i2c1_errors;
 
-#include "my_debug_servo.h"
-
 void i2c1_hw_init(void) {
 
   i2c1.reg_addr = I2C1;
@@ -634,40 +589,20 @@ void i2c1_hw_init(void) {
 
 }
 
-
 void i2c1_ev_irq_handler(void) {
-
-  uint32_t event = I2C_GetLastEvent(I2C1);
-  i2c_event(&i2c1, event);
-
+  i2c_event(&i2c1);
 }
 
-
 void i2c1_er_irq_handler(void) {
   i2c_error(&i2c1);
 }
 
 #endif /* USE_I2C1 */
 
-
-
-
-
 #ifdef USE_I2C2
 
-//  dec      hex
-//  196609   30001        BUSY  MSL |                 SB
-//  458882   70082    TRA BUSY  MSL | TXE       ADDR
-//  458884   70084    TRA BUSY  MSL | TXE  BTF
-//  196609   30001        BUSY  MSL |                 SB
-//  196610   30002        BUSY  MSL |           ADDR
-//
-
-
 struct i2c_errors i2c2_errors;
 
-#include "my_debug_servo.h"
-
 void i2c2_hw_init(void) {
 
   i2c2.reg_addr = I2C2;
@@ -721,72 +656,17 @@ void i2c2_hw_init(void) {
 
 
 void i2c2_ev_irq_handler(void) {
-  uint32_t event = I2C_GetLastEvent(I2C2);
-
-
-  i2c_event(&i2c2, event);
-
-  LED2_OFF();
-  LED1_OFF();
+  i2c_event(&i2c2);
 }
 
 void i2c2_er_irq_handler(void) {
   i2c_error(&i2c2);
-
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-
 }
 
 #endif /* USE_I2C2 */
 
+
+
 /////////////////////////////
 // User-space Interaction
 

From 46f4b37fa93532d41873dc8ab8a588ed298d3bf0 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 13:16:16 +0200
Subject: [PATCH 11/74] It compiles

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 142 ++++++++----------
 1 file changed, 66 insertions(+), 76 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index 2f137bc5da..d3902446d3 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -5,15 +5,18 @@
 #include <stm32/flash.h>
 #include <stm32/misc.h>
 
+
+/////////// DEBUGGING //////////////
+
 #define LEDSTART_ON() {}
 #define LEDSTART_OFF() {}
 
-static inline void LED1_ON()
+static inline void LED1_ON(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
 }
 
-static inline void LED1_OFF()
+static inline void LED1_OFF(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
 }
@@ -41,19 +44,14 @@ static inline void LED_INIT(void)
   LED2_OFF();
 }
 
+//////////////////////////////////////
 
-static void start_transaction(struct i2c_periph* p);
-static inline void end_of_transaction(struct i2c_periph *p);
 static inline void i2c_hard_reset(struct i2c_periph *p);
-static inline void i2c_reset_init(struct i2c_periph *p);
-
-#define I2C_BUSY 0x20
 
 #ifdef DEBUG_I2C
 #define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); }
 #define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); }
 #else
-//#define SPURIOUS_INTERRUPT(_periph, _status, _event) { periph->errors->unexpected_event_cnt++; abort_and_reset(_periph);}
 #define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);}
 #define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);}
 #endif
@@ -76,7 +74,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 10000
+      .I2C_ClockSpeed = 100000
 };
 #endif
 
@@ -90,26 +88,11 @@ static inline void i2c_apply_config(struct i2c_periph *p)
     I2C_Init(p->reg_addr, p->init_struct);
 }
 
-static inline void end_of_transaction(struct i2c_periph *p)
-{
-    p->trans_extract_idx++;
-    if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
-      p->trans_extract_idx = 0;
-    /* if we have no more transaction to process, stop here */
-    if (p->trans_extract_idx == p->trans_insert_idx)
-      p->status = I2CIdle;
-    /* if not, start next transaction */
-    else
-      start_transaction(p);
-}
-
 static inline void abort_and_reset(struct i2c_periph *p) {
     struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
     trans->status = I2CTransFailed;
-//    I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
     i2c_hard_reset(p);
-//    I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
-    end_of_transaction(p);
+    // TODO: Do something here
 }
 
 #ifdef USE_I2C2
@@ -122,54 +105,61 @@ static inline void abort_and_reset(struct i2c_periph *p) {
 
 // IDLE CHECK
 
-static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* p)
+static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* periph)
 {
-  return I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY) == RESET;
+  return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
 }
 
 // (RE)START
 
 static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
-  p->idx_buf = 0;
-  I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
-  I2C_GenerateSTART(p->reg_addr, ENABLE);
+  periph->idx_buf = 0;
+  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
+  I2C_GenerateSTART(periph->reg_addr, ENABLE);
 }
 
-static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* p) 
+static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph) 
 {
   /* if we have no more transaction to process, stop here */
-  if (p->trans_extract_idx == p->trans_insert_idx)
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
   {
-    p->status = I2CIdle;
+    // Should we disable just in case? normally not. So if more interrupts are 
+    // triggered there is a problem and we want to know.
+    // I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
+    periph->status = I2CIdle;
   }
   /* if not, start next transaction */
   else
   {
-    p->status = I2CStartRequested;
-    PPRZ_I2C_SEND_START(p);
+    periph->status = I2CStartRequested;
+    PPRZ_I2C_SEND_START(periph);
   }
 }
 
 static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 {
-  p->status = I2CRestartRequested;
-  PPRZ_I2C_SEND_START(p);
+  periph->status = I2CRestartRequested;
+  PPRZ_I2C_SEND_START(periph);
 }
 
-static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_trans *trans, I2CStatus _status)
+// STOP
+
+static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_transaction *trans, enum I2CTransactionStatus _status)
 {
   // Finish Current
   trans->status = _status;
  
   // When finished successfully the I2C_FLAG_MLS will be cleared after the stop condition was issued.
   // However: we do not need to wait for it to go the the next step, but if no stop condition was 
-  // sent than we are still talking to the same slave...
-
-  // todo: evaluate all possible routes to this function...
-  
-  // a) stop condition was already sheduled?
-  // b) stop condition not yet sheduled?
+  // sent yet than we are still talking to the same slave...
+  // When we are here all paths to this function with success have already issued a STOP, the others not.
+  // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+  if (_status != I2CTransSuccess)
+  {
+    // TODO: we might need to do much more here: see reset functions of antoine...
+    I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+  }  
 
   // Jump to the next
   periph->trans_extract_idx++;
@@ -182,21 +172,21 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
-static inline void i2c_event(struct i2c_periph *p)
+static inline void i2c_event(struct i2c_periph *periph)
 {
   // Referring to manual: 
   // -Doc ID 13902 Rev 11
 
 
   // Check to make sure that user space has an active transaction pending
-  if (p->trans_extract_idx == p->trans_insert_idx)
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
   {
     // no transaction?
-    p->errors->unexpected_event_cnt++;
+    periph->errors->unexpected_event_cnt++;
     return;
   }
 
-  struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
 
   /*	
 	There are 7 possible reasons to get here:
@@ -275,9 +265,17 @@ static inline void i2c_event(struct i2c_periph *p)
   //   quite fast: so we allow no other interrupt to be triggered in between
   //   reading the status and setting all needed flags
 
+
+  LED1_ON();
+
+
   __disable_irq();
   uint32_t event = I2C_GetLastEvent(periph->reg_addr);
 
+  if (event & I2C_FLAG_SB)
+  {
+    LED2_ON();
+  }
 
   ///////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////
@@ -286,7 +284,7 @@ static inline void i2c_event(struct i2c_periph *p)
   if (event & I2C_FLAG_SB)
   {
     // Periph was waiting for Start
-    if (p->status == I2CStartRequested)
+    if (periph->status == I2CStartRequested)
     {
       // Send Read Slave Address
       if(trans->type == I2CTransRx)
@@ -296,7 +294,7 @@ static inline void i2c_event(struct i2c_periph *p)
         I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
     }
     // Waiting for Restart: Always Rx
-    else if (p->status == I2CStartRequested)
+    else if (periph->status == I2CStartRequested)
     {
       I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
     }
@@ -304,7 +302,7 @@ static inline void i2c_event(struct i2c_periph *p)
     // status was not OK then the buf size is also bad
     else
     {
-      PPRZ_I2C_HAS_FINISHED(p, trans, I2CFailed);
+      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
     }
   }
 
@@ -318,7 +316,7 @@ static inline void i2c_event(struct i2c_periph *p)
   {
     // Do we have more data? and there is buffer room? 
     // (neglect BTF: if it was set it just means we were too slow)
-    while ((periph->idx_buf < trans->len_w) && (I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_TXE) == SET))
+    while ((periph->idx_buf < trans->len_w) && (I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_TXE) == SET))
     {
       I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
       periph->idx_buf++;
@@ -331,24 +329,25 @@ static inline void i2c_event(struct i2c_periph *p)
       } 
     }
 
-    // STM Manual Ev8_2 
     if ((event & I2C_FLAG_BTF) && (periph->idx_buf >= trans->len_w))
     {
       // Ready -> Stop
       if (trans->type == I2CTransTx) 
       {
-        PPRZ_I2C_HAS_FINISHED(p, trans, I2CSuccess);
+        // STM Manual Ev8_2 
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+        PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
       }
       // Rx/Trans -> Restart
       else 
       {
-        PPRZ_I2C_RESTART(p);
+        PPRZ_I2C_RESTART(periph);
       }
     }
     // If we had no more data but got no BTF then there is a problem
     else
     {
-      PPRZ_I2C_HAS_FINISHED(p, trans, I2CFailed);
+      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
     }
   }
 
@@ -373,7 +372,7 @@ static inline void i2c_event(struct i2c_periph *p)
     // This last byte has arrived
     if (periph->idx_buf >= trans->len_r) 
     {
-      PPRZ_I2C_HAS_FINISHED(p, trans, I2CSuccess);
+      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
     }
     // Tell the Slave it will be the last one
     else if (periph->idx_buf >= trans->len_r-1) 
@@ -393,12 +392,6 @@ static inline void i2c_event(struct i2c_periph *p)
   __enable_irq();
 
 
-  LED1_ON();
-
-  if (event & I2C_FLAG_TRA)
-  {
-    LED2_ON();
-  }
 
   LED2_OFF();
   LED1_OFF();
@@ -549,7 +542,8 @@ static inline void i2c_hard_reset(struct i2c_periph *p)
   
   i2c_apply_config(p);
   
-  if (regs->SR2 & I2C_BUSY) {
+  // Make sure the bus is free before resetting (p722)
+  if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
     // Reset the I2C block
     I2C_SoftwareResetCmd(p->reg_addr, ENABLE);
     I2C_SoftwareResetCmd(p->reg_addr, DISABLE);
@@ -640,17 +634,10 @@ void i2c2_hw_init(void) {
   /* Enable GPIOB clock */
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
 
-  /* Configure I2C1 pins: SCL and SDA ------------------------------------------*/
-  GPIO_InitTypeDef GPIO_InitStructure;
-  GPIO_StructInit(&GPIO_InitStructure);
-  GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_10 | GPIO_Pin_11;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
-  GPIO_Init(GPIOB, &GPIO_InitStructure);
-
   // Reset and initialize I2C HW
   i2c_reset_init(&i2c2);
 
+  // Extra
   LED_INIT();
 }
 
@@ -667,8 +654,8 @@ void i2c2_er_irq_handler(void) {
 
 
 
-/////////////////////////////
-// User-space Interaction
+/////////////////////////////////////////////////////////
+// Implement Interface Functions
 
 bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
 
@@ -680,14 +667,13 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
 
   t->status = I2CTransPending;
 
-
   __disable_irq();
   /* put transacation in queue */
   p->trans[p->trans_insert_idx] = t;
   p->trans_insert_idx = temp;
 
   /* if peripheral is idle, start the transaction */
-  if (PPRZ_I2C_IS_IDLE())
+  if (PPRZ_I2C_IS_IDLE(p))
     PPRZ_I2C_START_NEXT_TRANSACTION(p);
   /* else it will be started by the interrupt handler when the previous transactions completes */
   __enable_irq();
@@ -695,5 +681,9 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
   return TRUE;
 }
 
+bool_t i2c_idle(struct i2c_periph* p)
+{
+  return PPRZ_I2C_IS_IDLE(p);
+}
 
 

From 19c7254ffc36b8fd9564c752ba97e3c15fa0b0b0 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 15:08:15 +0200
Subject: [PATCH 12/74] Non-blocking HMC initialization...

---
 sw/airborne/peripherals/hmc5843.c | 89 ++++++++++++++++++-------------
 sw/airborne/peripherals/hmc5843.h |  2 -
 2 files changed, 53 insertions(+), 38 deletions(-)

diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index a0abba6f32..54f499e177 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -1,5 +1,7 @@
 #include "peripherals/hmc5843.h"
 
+#include "led.h"
+
 #define HMC5843_TIMEOUT 10
 
 #define bswap_16(x)   ((((x) & 0xFF00) >> 8) | (((x) & 0x00FF) << 8))
@@ -15,44 +17,63 @@ void hmc5843_init(void)
   hmc5843_arch_init();
 }
 
-// blocking, only intended to be called for initialization
-static void send_config(void)
+static void hmc_send_config(uint8_t _init)
 {
-  hmc5843.i2c_trans.type = I2CTransTx;
-  hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
-  hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
-  hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
-
-  hmc5843.i2c_trans.type = I2CTransTx;
-  hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
-  hmc5843.i2c_trans.buf[1] = 0x01<<5;
-  hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
-
-  hmc5843.i2c_trans.type = I2CTransTx;
-  hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
-  hmc5843.i2c_trans.buf[1] = 0x00;
-  hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
-
-
+  switch (_init)
+  {
+  case 0:
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
+    hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
+    hmc5843.i2c_trans.len_w = 2;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+    break;
+  case 1:
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
+    hmc5843.i2c_trans.buf[1] = 0x01<<5;
+    hmc5843.i2c_trans.len_w = 2;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  case 2:
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
+    hmc5843.i2c_trans.buf[1] = 0x00;
+    hmc5843.i2c_trans.len_w = 2;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  default: 
     hmc5843.i2c_trans.type = I2CTransTxRx;
     hmc5843.i2c_trans.len_r = 6;
     hmc5843.i2c_trans.len_w = 1;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
     i2c_submit(&i2c2, &hmc5843.i2c_trans);
+  }
 }
 
 void hmc5843_idle_task(void)
 {
+  if (hmc5843.timeout > HMC5843_TIMEOUT)
+  {
+    hmc5843.timeout = 0;
+    LED_TOGGLE(4);
+  }
 
   // Wait for I2C transaction object to be released by the I2C driver before changing anything
   if ((hmc5843.i2c_trans.status == I2CTransFailed) || (hmc5843.i2c_trans.status == I2CTransSuccess))
-{
+  {
+    if (hmc5843.initialized < 4)
+    {
+       if (hmc5843.i2c_trans.status == I2CTransSuccess)
+       {
+         hmc5843.initialized++;
+       }
+       hmc_send_config( hmc5843.initialized );
+     }
+     else
+     {
+
+ 	 
 /*
   // If transaction succeeded
   if (hmc5843.i2c_trans.status == I2CTransSuccess) 
@@ -67,24 +88,20 @@ void hmc5843_idle_task(void)
   // Start a new one
   if (hmc5843.timeout > HMC5843_TIMEOUT)
   {
+
     hmc5843.timeout = 0;
 */
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 6;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+      hmc5843.i2c_trans.type = I2CTransTxRx;
+      hmc5843.i2c_trans.len_r = 6;
+      hmc5843.i2c_trans.len_w = 1;
+      hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+      i2c_submit(&i2c2, &hmc5843.i2c_trans);
+    }
   }
 
 }
 
 void hmc5843_periodic(void)
 {
-  if (!hmc5843.initialized) {
-    send_config();
-    hmc5843.initialized = TRUE;
-    // hmc5843.i2c_trans.status = I2CTransSuccess;
-  }
-
   hmc5843.timeout++;
 }
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index 06b9ff1dcf..f3cd97515b 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -30,8 +30,6 @@
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
     uint32_t timeout;
-    uint8_t sent_tx;
-    uint8_t sent_rx;
     uint8_t initialized;
     uint8_t data_available;
   union {

From 00df45d0853f04e0388cc346b41e506c5283b274 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 16:12:26 +0200
Subject: [PATCH 13/74] Send all config messages

---
 sw/airborne/peripherals/hmc5843.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 54f499e177..1fc2b888c3 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -21,21 +21,21 @@ static void hmc_send_config(uint8_t _init)
 {
   switch (_init)
   {
-  case 0:
+  case 1:
     hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
     hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
     hmc5843.i2c_trans.len_w = 2;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
     break;
-  case 1:
+  case 2:
     hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
     hmc5843.i2c_trans.buf[1] = 0x01<<5;
     hmc5843.i2c_trans.len_w = 2;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
-  case 2:
+  case 3:
     hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
     hmc5843.i2c_trans.buf[1] = 0x00;

From 99be55b24a6115fa5f0caf6b8630f1cee86bb2d4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 16:13:37 +0200
Subject: [PATCH 14/74] Starting to work ...

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 310 ++++++++++--------
 1 file changed, 178 insertions(+), 132 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index d3902446d3..de73eb743f 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -26,7 +26,7 @@ static inline void LED2_ON()
   GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
 }
 
-static inline void LED2_OFF()
+static inline void LED2_OFF(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET );
 }
@@ -44,16 +44,30 @@ static inline void LED_INIT(void)
   LED2_OFF();
 }
 
-//////////////////////////////////////
+static inline void LED_STROBE2(void)
+{
+LED2_ON();
+LED2_OFF();
+LED1_ON();
+LED1_OFF();
+LED2_ON();
+LED2_OFF();
+LED1_ON();
+LED1_OFF();
+LED1_OFF();
+LED1_OFF();
+LED1_OFF();
 
-static inline void i2c_hard_reset(struct i2c_periph *p);
+}
+
+//////////////////////////////////////
 
 #ifdef DEBUG_I2C
 #define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); }
 #define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); }
 #else
-#define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);}
-#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);}
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { }
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { }
 #endif
 
 #ifdef USE_I2C1
@@ -74,26 +88,10 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 100000
+      .I2C_ClockSpeed = 30000
 };
 #endif
 
-static inline void i2c_delay(void)
-{
-    for (__IO int j = 0; j < 50; j++);
-}
-
-static inline void i2c_apply_config(struct i2c_periph *p)
-{
-    I2C_Init(p->reg_addr, p->init_struct);
-}
-
-static inline void abort_and_reset(struct i2c_periph *p) {
-    struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
-    trans->status = I2CTransFailed;
-    i2c_hard_reset(p);
-    // TODO: Do something here
-}
 
 #ifdef USE_I2C2
 
@@ -114,9 +112,11 @@ static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* periph)
 
 static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
+LED2_ON();
   periph->idx_buf = 0;
-  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE);
   I2C_GenerateSTART(periph->reg_addr, ENABLE);
+  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
+  I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
 }
 
 static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph) 
@@ -126,7 +126,7 @@ static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph)
   {
     // Should we disable just in case? normally not. So if more interrupts are 
     // triggered there is a problem and we want to know.
-    // I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
+    // I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
     periph->status = I2CIdle;
   }
   /* if not, start next transaction */
@@ -139,6 +139,8 @@ static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph)
 
 static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 {
+  // BTF is cleared by the stop condition only
+  I2C_ClearFlag(periph->reg_addr, I2C_FLAG_BTF);
   periph->status = I2CRestartRequested;
   PPRZ_I2C_SEND_START(periph);
 }
@@ -147,6 +149,11 @@ static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 
 static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_transaction *trans, enum I2CTransactionStatus _status)
 {
+//////////////////////////////////////////////
+        LED1_OFF();  //strobe
+        LED1_ON();
+//////////////////////////////////////////////
+
   // Finish Current
   trans->status = _status;
  
@@ -269,12 +276,16 @@ static inline void i2c_event(struct i2c_periph *periph)
   LED1_ON();
 
 
-  __disable_irq();
+  //__disable_irq();
   uint32_t event = I2C_GetLastEvent(periph->reg_addr);
 
   if (event & I2C_FLAG_SB)
   {
-    LED2_ON();
+    //LED2_ON();
+  }
+
+  if (event & I2C_FLAG_BTF)
+  {
   }
 
   ///////////////////////////////////////////////////////////////////////////////////
@@ -288,15 +299,21 @@ static inline void i2c_event(struct i2c_periph *periph)
     {
       // Send Read Slave Address
       if(trans->type == I2CTransRx)
+      {
         I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+      }
       // Send Write Slave Address
       else
+      {
         I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
+      }
+      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
     }
     // Waiting for Restart: Always Rx
     else if (periph->status == I2CStartRequested)
     {
       I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
     }
     // Problem: this problem need to be triggerd as if the 
     // status was not OK then the buf size is also bad
@@ -312,11 +329,11 @@ static inline void i2c_event(struct i2c_periph *periph)
   // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt 
   //     will be triggered until a start/stop occurs which can be quite long = many spurrious interrupts)
   // STM Manual Ev8
-  else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and not sending start/stop/addr
+  else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and no flag tra/start/stop/addr
   {
     // Do we have more data? and there is buffer room? 
     // (neglect BTF: if it was set it just means we were too slow)
-    while ((periph->idx_buf < trans->len_w) && (I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_TXE) == SET))
+    if (periph->idx_buf < trans->len_w)
     {
       I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
       periph->idx_buf++;
@@ -329,7 +346,7 @@ static inline void i2c_event(struct i2c_periph *periph)
       } 
     }
 
-    if ((event & I2C_FLAG_BTF) && (periph->idx_buf >= trans->len_w))
+    else if (event & I2C_FLAG_BTF)
     {
       // Ready -> Stop
       if (trans->type == I2CTransTx) 
@@ -341,6 +358,8 @@ static inline void i2c_event(struct i2c_periph *periph)
       // Rx/Trans -> Restart
       else 
       {
+
+
         PPRZ_I2C_RESTART(periph);
       }
     }
@@ -389,7 +408,7 @@ static inline void i2c_event(struct i2c_periph *periph)
   }
 
   // Now re-enable IRQ... it's been too long  
-  __enable_irq();
+  // __enable_irq();
 
 
 
@@ -399,168 +418,173 @@ static inline void i2c_event(struct i2c_periph *periph)
 
 }
 
-static inline void i2c_error(struct i2c_periph *p)
+static inline void i2c_error(struct i2c_periph *periph)
 {
-  p->errors->er_irq_cnt;
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
-    p->errors->ack_fail_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_AF);
+  periph->errors->er_irq_cnt;
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
+    periph->errors->ack_fail_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_AF);
   }
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
-    p->errors->miss_start_stop_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_BERR);
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
+    periph->errors->miss_start_stop_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BERR);
   }
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
-    p->errors->arb_lost_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_ARLO);
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
+    periph->errors->arb_lost_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_ARLO);
     //    I2C_AcknowledgeConfig(I2C2, DISABLE);
     //    uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
     //    I2C_GenerateSTOP(I2C2, ENABLE);
   }
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
-    p->errors->over_under_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_OVR);
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
+    periph->errors->over_under_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_OVR);
   }
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
-    p->errors->pec_recep_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_PECERR);
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
+    periph->errors->pec_recep_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_PECERR);
   }
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
-    p->errors->timeout_tlow_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_TIMEOUT);
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
+    periph->errors->timeout_tlow_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_TIMEOUT);
   }
-  if (I2C_GetITStatus(p->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
-    p->errors->smbus_alert_cnt++;
-    I2C_ClearITPendingBit(p->reg_addr, I2C_IT_SMBALERT);
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+    periph->errors->smbus_alert_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_SMBALERT);
   }
 
-  abort_and_reset(p);
+  // Check to make sure that user space has an active transaction pending
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    // no transaction?
+    periph->errors->unexpected_event_cnt++;
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+  PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
+ //abort_and_reset(p);
 
 
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
     LED1_ON();
     LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED1_ON();
-    LED1_OFF();
+    LED2_ON();
+    LED2_OFF();
 
 }
 
-
-static inline void i2c_hard_reset(struct i2c_periph *p)
+/*
+static inline void i2c_delay(void)
 {
-  I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr;
+    for (__IO int j = 0; j < 50; j++);
+}
 
-  I2C_DeInit(p->reg_addr);
 
+static inline void abort_and_reset(struct i2c_periph *periph) {
+    struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+    trans->status = I2CTransFailed;
+    i2c_hard_reset(periph);
+    // TODO: Do something here
+}
+
+static inline void i2c_hard_reset(struct i2c_periph *periph)
+{
   GPIO_InitTypeDef GPIO_InitStructure;
-  GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin;
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  I2C_DeInit(periph->reg_addr);
+
+  GPIO_InitStructure.GPIO_Pin = periph->scl_pin | periph->sda_pin;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
-  GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+  GPIO_SetBits(GPIOB, periph->scl_pin | periph->sda_pin);
   GPIO_Init(GPIOB, &GPIO_InitStructure);
 
-  while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) {
+  while(GPIO_ReadInputDataBit(GPIOB, periph->sda_pin) == Bit_RESET) {
     // Raise SCL, wait until SCL is high (in case of clock stretching)
-    GPIO_SetBits(GPIOB, p->scl_pin);
-    while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+    GPIO_SetBits(GPIOB, periph->scl_pin);
+    while (GPIO_ReadInputDataBit(GPIOB, periph->scl_pin) == Bit_RESET);
     i2c_delay();
     
     // Lower SCL, wait
-    GPIO_ResetBits(GPIOB, p->scl_pin);
+    GPIO_ResetBits(GPIOB, periph->scl_pin);
     i2c_delay();
     
     // Raise SCL, wait
-    GPIO_SetBits(GPIOB, p->scl_pin);
+    GPIO_SetBits(GPIOB, periph->scl_pin);
     i2c_delay();
   }
   
+
   // Generate a start condition followed by a stop condition
-  GPIO_SetBits(GPIOB, p->scl_pin);
+  GPIO_SetBits(GPIOB, periph->scl_pin);
   i2c_delay();
-  GPIO_ResetBits(GPIOB, p->sda_pin);
+  GPIO_ResetBits(GPIOB, periph->sda_pin);
   i2c_delay();
-  GPIO_ResetBits(GPIOB, p->sda_pin);
+  GPIO_ResetBits(GPIOB, periph->sda_pin);
   i2c_delay();
+
   
+
   // Raise both SCL and SDA and wait for SCL high (in case of clock stretching)
-  GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
-  while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+  GPIO_SetBits(GPIOB, periph->scl_pin | periph->sda_pin);
+  while (GPIO_ReadInputDataBit(GPIOB, periph->scl_pin) == Bit_RESET);
   
   // Wait for SDA to be high
-  while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET);
+  while (GPIO_ReadInputDataBit(GPIOB, periph->sda_pin) != Bit_SET);
   
   // SCL and SDA should be high at this point, bus should be free
   // Return the GPIO pins to the alternate function
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
   GPIO_Init(GPIOB, &GPIO_InitStructure);
   
-  I2C_DeInit(p->reg_addr);
+
+  I2C_DeInit(periph->reg_addr);
   
-  i2c_apply_config(p);
+  i2c_apply_config(periph);
   
+
   // Make sure the bus is free before resetting (p722)
   if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
     // Reset the I2C block
-    I2C_SoftwareResetCmd(p->reg_addr, ENABLE);
-    I2C_SoftwareResetCmd(p->reg_addr, DISABLE);
+    I2C_SoftwareResetCmd(periph->reg_addr, ENABLE);
+    I2C_SoftwareResetCmd(periph->reg_addr, DISABLE);
   }
+
 }
-
-static inline void i2c_reset_init(struct i2c_periph *p)
-{
-  // Reset bus and configure GPIO pins
-  i2c_hard_reset(p);
-
-  // enable peripheral
-  I2C_Cmd(p->reg_addr, ENABLE);
-
-  // enable error interrupts
-  I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE);
-}
+*/
 #endif /* USE_I2C2 */
 
 
@@ -581,6 +605,8 @@ void i2c1_hw_init(void) {
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c1_errors);
 
+  // Extra
+  LED_INIT();
 }
 
 void i2c1_ev_irq_handler(void) {
@@ -609,7 +635,7 @@ void i2c2_hw_init(void) {
   ZEROS_ERR_COUNTER(i2c2_errors);
 
   /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
-  I2C_DeInit(I2C2);
+  //I2C_DeInit(I2C2);
 
   NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
   NVIC_InitTypeDef  NVIC_InitStructure;
@@ -634,11 +660,29 @@ void i2c2_hw_init(void) {
   /* Enable GPIOB clock */
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
 
-  // Reset and initialize I2C HW
-  i2c_reset_init(&i2c2);
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c2.scl_pin | i2c2.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C2);
+
+  // enable peripheral
+  I2C_Cmd(I2C2, ENABLE);
+
+  I2C_Init(I2C2, i2c2.init_struct);
+
+
+//  I2C_SoftwareResetCmd(I2C2, ENABLE);
+//  I2C_SoftwareResetCmd(I2C2, DISABLE);
+
+  // Reset and initialize I2C HW
+  // enable error interrupts
+  I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
+
+//  i2c_reset_init(&i2c2);
 
-  // Extra
-  LED_INIT();
 }
 
 
@@ -657,33 +701,35 @@ void i2c2_er_irq_handler(void) {
 /////////////////////////////////////////////////////////
 // Implement Interface Functions
 
-bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
+bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
   uint8_t temp;
-  temp = p->trans_insert_idx + 1;
+  temp = periph->trans_insert_idx + 1;
   if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
-  if (temp == p->trans_extract_idx)
+  if (temp == periph->trans_extract_idx)
     return FALSE;                          // queue full
 
   t->status = I2CTransPending;
 
   __disable_irq();
   /* put transacation in queue */
-  p->trans[p->trans_insert_idx] = t;
-  p->trans_insert_idx = temp;
+  periph->trans[periph->trans_insert_idx] = t;
+  periph->trans_insert_idx = temp;
 
   /* if peripheral is idle, start the transaction */
-  if (PPRZ_I2C_IS_IDLE(p))
-    PPRZ_I2C_START_NEXT_TRANSACTION(p);
+  // if (PPRZ_I2C_IS_IDLE(p))
+  if (periph->status == I2CIdle)
+    PPRZ_I2C_START_NEXT_TRANSACTION(periph);
   /* else it will be started by the interrupt handler when the previous transactions completes */
   __enable_irq();
 
   return TRUE;
 }
 
-bool_t i2c_idle(struct i2c_periph* p)
+bool_t i2c_idle(struct i2c_periph* periph)
 {
-  return PPRZ_I2C_IS_IDLE(p);
+  //return PPRZ_I2C_IS_IDLE(periph);
+  return periph->status == I2CIdle;
 }
 
 

From 8f0fc097cd205e7e011da6678ae01c84c91328d9 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Jul 2011 18:21:23 +0200
Subject: [PATCH 15/74] Extra testing

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 124 +++---------------
 1 file changed, 21 insertions(+), 103 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index de73eb743f..4f9ebd5f61 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -5,12 +5,10 @@
 #include <stm32/flash.h>
 #include <stm32/misc.h>
 
+//#include "led.h" 
 
 /////////// DEBUGGING //////////////
 
-#define LEDSTART_ON() {}
-#define LEDSTART_OFF() {}
-
 static inline void LED1_ON(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
@@ -88,7 +86,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 30000
+      .I2C_ClockSpeed = 400000
 };
 #endif
 
@@ -112,7 +110,6 @@ static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* periph)
 
 static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
-LED2_ON();
   periph->idx_buf = 0;
   I2C_GenerateSTART(periph->reg_addr, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
@@ -139,8 +136,10 @@ static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph)
 
 static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 {
-  // BTF is cleared by the stop condition only
-  I2C_ClearFlag(periph->reg_addr, I2C_FLAG_BTF);
+LED2_ON();
+//        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+//      I2C_SendData(periph->reg_addr, 0);
+//  I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BTF);
   periph->status = I2CRestartRequested;
   PPRZ_I2C_SEND_START(periph);
 }
@@ -149,11 +148,6 @@ static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 
 static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_transaction *trans, enum I2CTransactionStatus _status)
 {
-//////////////////////////////////////////////
-        LED1_OFF();  //strobe
-        LED1_ON();
-//////////////////////////////////////////////
-
   // Finish Current
   trans->status = _status;
  
@@ -275,18 +269,13 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   LED1_ON();
 
+  //I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
   //__disable_irq();
   uint32_t event = I2C_GetLastEvent(periph->reg_addr);
 
-  if (event & I2C_FLAG_SB)
-  {
-    //LED2_ON();
-  }
+  //uint32_t event = (((uint32_t)(regs->SR2)) << 16) + regs->SR1;
 
-  if (event & I2C_FLAG_BTF)
-  {
-  }
 
   ///////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////
@@ -310,7 +299,7 @@ static inline void i2c_event(struct i2c_periph *periph)
       I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
     }
     // Waiting for Restart: Always Rx
-    else if (periph->status == I2CStartRequested)
+    else if (periph->status == I2CRestartRequested)
     {
       I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
       I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
@@ -343,6 +332,11 @@ static inline void i2c_event(struct i2c_periph *periph)
       if (periph->idx_buf >= trans->len_w)
       {
         I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+        // If this is followed by a restart: then we need to set the startbit to avoid extra interrupts.
+        if (trans->type != I2CTransTx) 
+        {
+          PPRZ_I2C_RESTART(periph);
+        }
       } 
     }
 
@@ -355,13 +349,8 @@ static inline void i2c_event(struct i2c_periph *periph)
         I2C_GenerateSTOP(periph->reg_addr, ENABLE);
         PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
       }
-      // Rx/Trans -> Restart
-      else 
-      {
-
-
-        PPRZ_I2C_RESTART(periph);
-      }
+      // Rx/Trans -> Restart: 
+      // Do not wait for BTF
     }
     // If we had no more data but got no BTF then there is a problem
     else
@@ -505,86 +494,16 @@ static inline void i2c_error(struct i2c_periph *periph)
 
 }
 
+  
 /*
-static inline void i2c_delay(void)
-{
-    for (__IO int j = 0; j < 50; j++);
-}
-
-
-static inline void abort_and_reset(struct i2c_periph *periph) {
-    struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
-    trans->status = I2CTransFailed;
-    i2c_hard_reset(periph);
-    // TODO: Do something here
-}
-
-static inline void i2c_hard_reset(struct i2c_periph *periph)
-{
-  GPIO_InitTypeDef GPIO_InitStructure;
-  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-
-  I2C_DeInit(periph->reg_addr);
-
-  GPIO_InitStructure.GPIO_Pin = periph->scl_pin | periph->sda_pin;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
-  GPIO_SetBits(GPIOB, periph->scl_pin | periph->sda_pin);
-  GPIO_Init(GPIOB, &GPIO_InitStructure);
-
-  while(GPIO_ReadInputDataBit(GPIOB, periph->sda_pin) == Bit_RESET) {
-    // Raise SCL, wait until SCL is high (in case of clock stretching)
-    GPIO_SetBits(GPIOB, periph->scl_pin);
-    while (GPIO_ReadInputDataBit(GPIOB, periph->scl_pin) == Bit_RESET);
-    i2c_delay();
-    
-    // Lower SCL, wait
-    GPIO_ResetBits(GPIOB, periph->scl_pin);
-    i2c_delay();
-    
-    // Raise SCL, wait
-    GPIO_SetBits(GPIOB, periph->scl_pin);
-    i2c_delay();
-  }
-  
-
-  // Generate a start condition followed by a stop condition
-  GPIO_SetBits(GPIOB, periph->scl_pin);
-  i2c_delay();
-  GPIO_ResetBits(GPIOB, periph->sda_pin);
-  i2c_delay();
-  GPIO_ResetBits(GPIOB, periph->sda_pin);
-  i2c_delay();
-
-  
-
-  // Raise both SCL and SDA and wait for SCL high (in case of clock stretching)
-  GPIO_SetBits(GPIOB, periph->scl_pin | periph->sda_pin);
-  while (GPIO_ReadInputDataBit(GPIOB, periph->scl_pin) == Bit_RESET);
-  
-  // Wait for SDA to be high
-  while (GPIO_ReadInputDataBit(GPIOB, periph->sda_pin) != Bit_SET);
-  
-  // SCL and SDA should be high at this point, bus should be free
-  // Return the GPIO pins to the alternate function
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
-  GPIO_Init(GPIOB, &GPIO_InitStructure);
-  
-
-  I2C_DeInit(periph->reg_addr);
-  
-  i2c_apply_config(periph);
-  
-
   // Make sure the bus is free before resetting (p722)
   if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
     // Reset the I2C block
     I2C_SoftwareResetCmd(periph->reg_addr, ENABLE);
     I2C_SoftwareResetCmd(periph->reg_addr, DISABLE);
   }
-
-}
 */
+
 #endif /* USE_I2C2 */
 
 
@@ -662,7 +581,7 @@ void i2c2_hw_init(void) {
 
   GPIO_InitTypeDef GPIO_InitStructure;
   GPIO_InitStructure.GPIO_Pin = i2c2.scl_pin | i2c2.sda_pin;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
   GPIO_Init(GPIOB, &GPIO_InitStructure);
 
@@ -673,7 +592,6 @@ void i2c2_hw_init(void) {
 
   I2C_Init(I2C2, i2c2.init_struct);
 
-
 //  I2C_SoftwareResetCmd(I2C2, ENABLE);
 //  I2C_SoftwareResetCmd(I2C2, DISABLE);
 
@@ -728,8 +646,8 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
 bool_t i2c_idle(struct i2c_periph* periph)
 {
-  //return PPRZ_I2C_IS_IDLE(periph);
-  return periph->status == I2CIdle;
+  return PPRZ_I2C_IS_IDLE(periph);
+  //return periph->status == I2CIdle;
 }
 
 

From 2784ebc51090beda122e1b8c4b51ccbc000e61a4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Sat, 9 Jul 2011 01:30:59 +0200
Subject: [PATCH 16/74] Stop thinking bytes. no: think performance: this means
 the driver must think 2 steps ahead.

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 24 +++++++++++++++----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index 4f9ebd5f61..53fbb9f2ff 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -86,7 +86,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 400000
+      .I2C_ClockSpeed = 30000
 };
 #endif
 
@@ -136,7 +136,7 @@ static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph)
 
 static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
 {
-LED2_ON();
+//LED2_ON();
 //        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
 //      I2C_SendData(periph->reg_addr, 0);
 //  I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BTF);
@@ -276,6 +276,11 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   //uint32_t event = (((uint32_t)(regs->SR2)) << 16) + regs->SR1;
 
+  if (event & I2C_FLAG_TXE)
+  {
+    LED2_ON();
+  }
+
 
   ///////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////
@@ -310,6 +315,7 @@ static inline void i2c_event(struct i2c_periph *periph)
     {
       PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
     }
+    periph->status = I2CAddrWrSent;
   }
 
   ///////////////////////////////////////////////////////////////////////////////////
@@ -320,9 +326,15 @@ static inline void i2c_event(struct i2c_periph *periph)
   // STM Manual Ev8
   else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and no flag tra/start/stop/addr
   {
+    if (periph->status == I2CRestartRequested)
+    {
+      // Neglect this interrupt: We just issued the restart last session already
+          PPRZ_I2C_RESTART(periph);
+    }
+
     // Do we have more data? and there is buffer room? 
     // (neglect BTF: if it was set it just means we were too slow)
-    if (periph->idx_buf < trans->len_w)
+    else if (periph->idx_buf < trans->len_w)
     {
       I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
       periph->idx_buf++;
@@ -335,7 +347,7 @@ static inline void i2c_event(struct i2c_periph *periph)
         // If this is followed by a restart: then we need to set the startbit to avoid extra interrupts.
         if (trans->type != I2CTransTx) 
         {
-          PPRZ_I2C_RESTART(periph);
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
         }
       } 
     }
@@ -451,7 +463,6 @@ static inline void i2c_error(struct i2c_periph *periph)
   }
 
   struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
-  PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
  //abort_and_reset(p);
 
 
@@ -492,6 +503,9 @@ static inline void i2c_error(struct i2c_periph *periph)
     LED2_ON();
     LED2_OFF();
 
+  PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
+
+
 }
 
   

From 5d8c505617af48d2ccb51ed426f706242a38caff Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 12 Jul 2011 14:49:11 +0200
Subject: [PATCH 17/74] I2C Driver that bypasses libSTM: managed to get rid of
 unwanted (but logic) BTF interrupt

---
 conf/autopilot/rotorcraft.makefile            |   2 +-
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      |  66 +-
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 736 ++++++++++++++++++
 sw/airborne/peripherals/hmc5843.c             |  22 +-
 4 files changed, 786 insertions(+), 40 deletions(-)
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c

diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
index 47b3922eb2..e839290903 100644
--- a/conf/autopilot/rotorcraft.makefile
+++ b/conf/autopilot/rotorcraft.makefile
@@ -102,7 +102,7 @@ ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 # I2C is needed for speed controllers and barometers on lisa
 ap.srcs += mcu_periph/i2c.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.cdw.c
+ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.cdw.nolib.c
 
 ap.srcs += $(SRC_FIRMWARE)/commands.c
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index 53fbb9f2ff..fd1ffb3e20 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -42,6 +42,21 @@ static inline void LED_INIT(void)
   LED2_OFF();
 }
 
+
+static inline void LED_ERROR(uint8_t nr)
+{
+  for (int i=0;i<20;i++)
+  {
+    LED1_ON();
+    LED1_OFF();
+    if (nr == i)
+      LED2_OFF();
+    else
+      LED2_ON();
+    LED2_OFF();    
+  }
+}
+
 static inline void LED_STROBE2(void)
 {
 LED2_ON();
@@ -86,7 +101,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 30000
+      .I2C_ClockSpeed = 300000
 };
 #endif
 
@@ -421,14 +436,17 @@ static inline void i2c_event(struct i2c_periph *periph)
 
 static inline void i2c_error(struct i2c_periph *periph)
 {
+  uint8_t err_nr = 0;
   periph->errors->er_irq_cnt;
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
     periph->errors->ack_fail_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_AF);
+    err_nr = 1;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
     periph->errors->miss_start_stop_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BERR);
+    err_nr = 2;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
     periph->errors->arb_lost_cnt++;
@@ -436,29 +454,39 @@ static inline void i2c_error(struct i2c_periph *periph)
     //    I2C_AcknowledgeConfig(I2C2, DISABLE);
     //    uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
     //    I2C_GenerateSTOP(I2C2, ENABLE);
+    err_nr = 3;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
     periph->errors->over_under_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_OVR);
+    err_nr = 4;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
     periph->errors->pec_recep_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_PECERR);
+    err_nr = 5;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
     periph->errors->timeout_tlow_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_TIMEOUT);
+    err_nr = 6;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
     periph->errors->smbus_alert_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_SMBALERT);
+    err_nr = 7;
   }
 
+
+  LED_ERROR(err_nr);
+
+
   // Check to make sure that user space has an active transaction pending
   if (periph->trans_extract_idx == periph->trans_insert_idx)
   {
     // no transaction?
     periph->errors->unexpected_event_cnt++;
+    err_nr = 8;
     return;
   }
 
@@ -466,42 +494,6 @@ static inline void i2c_error(struct i2c_periph *periph)
  //abort_and_reset(p);
 
 
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
-    LED1_ON();
-    LED1_OFF();
-    LED2_ON();
-    LED2_OFF();
 
   PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
new file mode 100644
index 0000000000..60f70461d9
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -0,0 +1,736 @@
+#include "mcu_periph/i2c.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+
+//#include "led.h" 
+
+/////////// DEBUGGING //////////////
+
+static inline void LED1_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
+}
+
+static inline void LED1_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
+}
+
+static inline void LED2_ON()
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
+}
+
+static inline void LED2_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET );
+}
+
+static inline void LED_INIT(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  LED1_OFF();
+  LED2_OFF();
+}
+
+
+static inline void LED_ERROR(uint8_t nr)
+{
+  for (int i=0;i<20;i++)
+  {
+    LED1_ON();
+    LED1_OFF();
+    if (nr == i)
+      LED2_OFF();
+    else
+      LED2_ON();
+    LED2_OFF();    
+  }
+}
+
+static inline void LED_STROBE2(void)
+{
+LED2_ON();
+LED2_OFF();
+LED1_ON();
+LED1_OFF();
+LED2_ON();
+LED2_OFF();
+LED1_ON();
+LED1_OFF();
+LED1_OFF();
+LED1_OFF();
+LED1_OFF();
+
+}
+
+//////////////////////////////////////
+
+#ifdef DEBUG_I2C
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); }
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); }
+#else
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { }
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { }
+#endif
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef  I2C1_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 200000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef  I2C2_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 30000
+};
+#endif
+
+
+#ifdef USE_I2C2
+
+
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// IDLE CHECK
+
+static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* periph)
+{
+  return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
+}
+
+// (RE)START
+
+static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
+{
+  periph->idx_buf = 0;
+  I2C_GenerateSTART(periph->reg_addr, ENABLE);
+  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
+  I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+}
+
+static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph) 
+{
+  /* if we have no more transaction to process, stop here */
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    // Should we disable just in case? normally not. So if more interrupts are 
+    // triggered there is a problem and we want to know.
+    // I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
+    periph->status = I2CIdle;
+  }
+  /* if not, start next transaction */
+  else
+  {
+    periph->status = I2CStartRequested;
+    PPRZ_I2C_SEND_START(periph);
+  }
+}
+
+static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
+{
+//LED2_ON();
+//        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+//      I2C_SendData(periph->reg_addr, 0);
+//  I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BTF);
+  periph->status = I2CRestartRequested;
+  PPRZ_I2C_SEND_START(periph);
+}
+
+// STOP
+
+static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_transaction *trans, enum I2CTransactionStatus _status)
+{
+  // Finish Current
+  trans->status = _status;
+ 
+  // When finished successfully the I2C_FLAG_MLS will be cleared after the stop condition was issued.
+  // However: we do not need to wait for it to go the the next step, but if no stop condition was 
+  // sent yet than we are still talking to the same slave...
+  // When we are here all paths to this function with success have already issued a STOP, the others not.
+  // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+  if (_status != I2CTransSuccess)
+  {
+    // TODO: we might need to do much more here: see reset functions of antoine...
+    I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+  }  
+
+  // Jump to the next
+  periph->trans_extract_idx++;
+  if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+    periph->trans_extract_idx = 0;
+
+  PPRZ_I2C_START_NEXT_TRANSACTION(periph);
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// Status Register 1
+
+#define I2C_SR1_BIT_SB			(1<<0)		// Start Condition Met
+#define I2C_SR1_BIT_ADDR		(1<<1)		// Address Sent
+#define I2C_SR1_BIT_BTF			(1<<2)		// SCL held low
+#define I2C_SR1_BIT_RXNE		(1<<6)		// Data Read Available
+#define I2C_SR1_BIT_TXE			(1<<7)		// TX buffer space available
+
+#define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop
+#define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure
+
+#define I2C_SR1_BITS_ERR		(I2C_SR_BIT_ERR_BUS|I2C_SR_BIT_ERR_AF)
+
+// Status Register 2
+
+#define I2C_SR2_BIT_TRA			(1<<2)		// Transmitting
+#define I2C_SR2_BIT_BUSY		(1<<1)		// Busy
+#define I2C_SR2_BIT_MSL			(1<<0)		// Master Selected
+
+// Control Register 1
+
+#define I2C_CR1_BIT_PE			(1<<0)		// Peripheral Enable
+#define I2C_CR1_BIT_START		(1<<8)		// Generate a Start
+#define I2C_CR1_BIT_STOP		(1<<9)		// Generate a Stop
+#define I2C_CR1_BIT_ACK			(1<<10)		// ACK / NACK
+#define I2C_CR1_BIT_POS			(1<<11)		// Ack will control not the next but secondnext received byte
+#define I2C_CR1_BIT_SWRST		(1<<15)		// Clear Busy Condition when no stop was detected
+
+// Control Register 2
+
+#define I2C_CR2_BIT_ITERREN		(1<<8)		// Error Interrupt
+#define I2C_CR2_BIT_ITEVTEN		(1<<9)		// Event Interrupt
+#define I2C_CR2_BIT_ITBUFEN		(1<<10)		// Buffer Interrupt
+
+
+// Bit Control
+
+#define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
+
+static inline void i2c_event(struct i2c_periph *periph)
+{
+  /*	
+	There are 7 possible reasons to get here:
+
+	If IT_EV_FEN
+	-------------------------
+
+	We are always interested in all IT_EV_FEV: all are required.
+
+	1) SB		// Start Condition Success in Master mode
+	2) ADDR		// Address sent received Acknoledge
+	[3 ADDR10]	// -- 10bit address stuff
+	[4 STOPF]	// -- only for slaves: master has no stop interrupt
+	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
+
+	// Beware: using the buffered I2C has some interesting properties:
+	  -when receiving BTF only occurs after the 2nd received byte: after the first byte is received it is 
+           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost)
+	  -when transmitting, and writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C serializer and the buffer is ready for more. You can already
+	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+	    
+	// Beware: the order in which Status is read determines how flags are cleared.
+
+	If IT_EV_FEN AND IT_EV_BUF
+	--------------------------
+
+	We are always interested in buffer interrupts IT_EV_BUF except in transmission when all data was sent
+
+	6) RxNE
+	7) TxE
+
+	--------------------------------------------------------------------------------------------------
+	// This driver uses only a subset of the pprz_i2c_states for several reasons:
+	// -we have less interrupts than the I2CStatus states (for efficiency)
+	// -status register flags better correspond to reality, especially in case of I2C errors
+
+	enum I2CStatus {
+	  I2CIdle,			// Dummy: Actual I2C Peripheral idle detection is safer with the 
+					// hardware status register flag I2C_FLAG_BUSY.
+
+	  I2CStartRequested,		// EV5: used to differentiate S en Sr
+	  I2CRestartRequested,		// EV5: used to differentiate S en Sr
+
+	  I2CSendingByte,		// Not used: using the hardware status reg I2C_FLAG_TRA
+	  I2CReadingByte,
+	  I2CAddrWrSent,		// Since we can do many things at once and we
+	  I2CAddrRdSent,		// have buffered sending, these states
+	  I2CSendingLastByte,		// do not correspond to the real state of the
+	  I2CReadingLastByte,		// STM I2C driver so they are not used
+	  I2CStopRequested,		
+
+	  I2CComplete,			// Used to provide the result
+	  I2CFailed
+	};
+
+	---------
+
+	The STM waits (holding SCL low) for user interaction:
+	a) after a master-start (waiting for address)
+	b) after an address (waiting for data)
+	   not during data sending when using buffered
+	c) after the last byte is transmitted (waiting for either stop or restart)
+	   not during data receiving when using buffered
+	   not after the last byte is received
+
+   */
+
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  // Reading the status:
+  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
+  //   the transmission of the byte is finished. At higher clock rates that can be
+  //   quite fast: so we allow no other interrupt to be triggered in between
+  //   reading the status and setting all needed flags
+
+  // Direct Access to the I2C Registers
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  volatile uint16_t SR1 = regs->SR1;
+  volatile uint16_t SR2 = regs->SR2;
+  
+  LED1_ON();
+  LED1_OFF();
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = 0x3C;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    regs->DR = 0x00;
+    regs->DR = 0x18;
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
+  {
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Also provide some dummy data in DR to silent the BTF interrupt
+    regs->DR = 0x00;
+
+    // After the stop: start again
+    regs->CR1 |= I2C_CR1_BIT_START;
+  }
+
+  return;
+
+  // Referring to manual: 
+  // -Doc ID 13902 Rev 11
+
+
+  // Check to make sure that user space has an active transaction pending
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    // no transaction?
+    periph->errors->unexpected_event_cnt++;
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+
+
+  LED1_ON();
+
+  //I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  //__disable_irq();
+  uint32_t event = I2C_GetLastEvent(periph->reg_addr);
+
+  //uint32_t event = (((uint32_t)(regs->SR2)) << 16) + regs->SR1;
+
+  if (event & I2C_FLAG_TXE)
+  {
+    LED2_ON();
+  }
+
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////
+  // START: Start Condition in Master Mode: 
+  // STM Manual Ev5
+  if (event & I2C_FLAG_SB)
+  {
+    // Periph was waiting for Start
+    if (periph->status == I2CStartRequested)
+    {
+      // Send Read Slave Address
+      if(trans->type == I2CTransRx)
+      {
+        I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+      }
+      // Send Write Slave Address
+      else
+      {
+        I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
+      }
+      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+    }
+    // Waiting for Restart: Always Rx
+    else if (periph->status == I2CRestartRequested)
+    {
+      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+    }
+    // Problem: this problem need to be triggerd as if the 
+    // status was not OK then the buf size is also bad
+    else
+    {
+      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
+    }
+    periph->status = I2CAddrWrSent;
+  }
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////
+  // TRANSMIT: Buffer Can accept the next byte for transmission
+  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt 
+  //     will be triggered until a start/stop occurs which can be quite long = many spurrious interrupts)
+  // STM Manual Ev8
+  else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and no flag tra/start/stop/addr
+  {
+    if (periph->status == I2CRestartRequested)
+    {
+      // Neglect this interrupt: We just issued the restart last session already
+          PPRZ_I2C_RESTART(periph);
+    }
+
+    // Do we have more data? and there is buffer room? 
+    // (neglect BTF: if it was set it just means we were too slow)
+    else if (periph->idx_buf < trans->len_w)
+    {
+      I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
+      periph->idx_buf++;
+      // Was this one the Last? -> Disable the buf interrupt (until next start) and wait for BTF
+      // -we could gain a bit of efficiency by already starting the next action but for 
+      //  code-readability we will wait until the last tx-byte is sent
+      if (periph->idx_buf >= trans->len_w)
+      {
+        I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+        // If this is followed by a restart: then we need to set the startbit to avoid extra interrupts.
+        if (trans->type != I2CTransTx) 
+        {
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+        }
+      } 
+    }
+
+    else if (event & I2C_FLAG_BTF)
+    {
+      // Ready -> Stop
+      if (trans->type == I2CTransTx) 
+      {
+        // STM Manual Ev8_2 
+        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+        PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
+      }
+      // Rx/Trans -> Restart: 
+      // Do not wait for BTF
+    }
+    // If we had no more data but got no BTF then there is a problem
+    else
+    {
+      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
+    }
+  }
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////
+  // RECEIVE:
+  // while receiving: the master needs to signal to the slave if more data is needed
+  else if ((event & I2C_FLAG_ADDR) || (event & I2C_FLAG_RXNE))
+  {
+    // data is available every time RXNE is set. If BTF is set it means that 2 bytes are 
+    // ready to read (one in shift register) and I2C has stopped until the buffer can accept new data.
+    if (event & I2C_FLAG_RXNE) 
+    {
+      uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
+      if (periph->idx_buf < trans->len_r) 
+      {
+        trans->buf[periph->idx_buf] = read_byte;
+        periph->idx_buf++;
+      }
+    }
+
+    // This last byte has arrived
+    if (periph->idx_buf >= trans->len_r) 
+    {
+      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
+    }
+    // Tell the Slave it will be the last one
+    else if (periph->idx_buf >= trans->len_r-1) 
+    {
+      I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);         // give them a nack once it's done
+      I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and follow with a stop
+    }
+    // Ask the Slave to send more
+    else
+    {
+      I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);
+    }
+
+  }
+
+  // Now re-enable IRQ... it's been too long  
+  // __enable_irq();
+
+
+
+  LED2_OFF();
+  LED1_OFF();
+
+
+}
+
+static inline void i2c_error(struct i2c_periph *periph)
+{
+  uint8_t err_nr = 0;
+  periph->errors->er_irq_cnt;
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
+    periph->errors->ack_fail_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_AF);
+    err_nr = 1;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
+    periph->errors->miss_start_stop_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BERR);
+    err_nr = 2;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
+    periph->errors->arb_lost_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_ARLO);
+    //    I2C_AcknowledgeConfig(I2C2, DISABLE);
+    //    uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
+    //    I2C_GenerateSTOP(I2C2, ENABLE);
+    err_nr = 3;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
+    periph->errors->over_under_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_OVR);
+    err_nr = 4;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
+    periph->errors->pec_recep_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_PECERR);
+    err_nr = 5;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
+    periph->errors->timeout_tlow_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_TIMEOUT);
+    err_nr = 6;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+    periph->errors->smbus_alert_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_SMBALERT);
+    err_nr = 7;
+  }
+
+
+  LED_ERROR(err_nr);
+
+
+  // Check to make sure that user space has an active transaction pending
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    // no transaction?
+    periph->errors->unexpected_event_cnt++;
+    err_nr = 8;
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+ //abort_and_reset(p);
+
+
+
+  PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
+
+
+}
+
+  
+/*
+  // Make sure the bus is free before resetting (p722)
+  if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
+    // Reset the I2C block
+    I2C_SoftwareResetCmd(periph->reg_addr, ENABLE);
+    I2C_SoftwareResetCmd(periph->reg_addr, DISABLE);
+  }
+*/
+
+#endif /* USE_I2C2 */
+
+
+
+
+#ifdef USE_I2C1
+
+struct i2c_errors i2c1_errors;
+
+void i2c1_hw_init(void) {
+
+  i2c1.reg_addr = I2C1;
+  i2c1.init_struct = &I2C1_InitStruct;
+  i2c1.scl_pin = GPIO_Pin_6;
+  i2c1.sda_pin = GPIO_Pin_7;
+  i2c1.errors = &i2c1_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c1_errors);
+
+  // Extra
+  LED_INIT();
+}
+
+void i2c1_ev_irq_handler(void) {
+  i2c_event(&i2c1);
+}
+
+void i2c1_er_irq_handler(void) {
+  i2c_error(&i2c1);
+}
+
+#endif /* USE_I2C1 */
+
+#ifdef USE_I2C2
+
+struct i2c_errors i2c2_errors;
+
+void i2c2_hw_init(void) {
+
+  i2c2.reg_addr = I2C2;
+  i2c2.init_struct = &I2C2_InitStruct;
+  i2c2.scl_pin = GPIO_Pin_10;
+  i2c2.sda_pin = GPIO_Pin_11;
+  i2c2.errors = &i2c2_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c2_errors);
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C2);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C2 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C2 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C2 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c2.scl_pin | i2c2.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C2);
+
+  // enable peripheral
+  I2C_Cmd(I2C2, ENABLE);
+
+  I2C_Init(I2C2, i2c2.init_struct);
+
+//  I2C_SoftwareResetCmd(I2C2, ENABLE);
+//  I2C_SoftwareResetCmd(I2C2, DISABLE);
+
+  // Reset and initialize I2C HW
+  // enable error interrupts
+  I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
+
+//  i2c_reset_init(&i2c2);
+
+}
+
+
+void i2c2_ev_irq_handler(void) {
+  i2c_event(&i2c2);
+}
+
+void i2c2_er_irq_handler(void) {
+  i2c_error(&i2c2);
+}
+
+#endif /* USE_I2C2 */
+
+
+
+/////////////////////////////////////////////////////////
+// Implement Interface Functions
+
+bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
+
+  uint8_t temp;
+  temp = periph->trans_insert_idx + 1;
+  if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
+  if (temp == periph->trans_extract_idx)
+    return FALSE;                          // queue full
+
+  t->status = I2CTransPending;
+
+  __disable_irq();
+  /* put transacation in queue */
+  periph->trans[periph->trans_insert_idx] = t;
+  periph->trans_insert_idx = temp;
+
+  /* if peripheral is idle, start the transaction */
+  // if (PPRZ_I2C_IS_IDLE(p))
+  if (periph->status == I2CIdle)
+    PPRZ_I2C_START_NEXT_TRANSACTION(periph);
+  /* else it will be started by the interrupt handler when the previous transactions completes */
+  __enable_irq();
+
+  return TRUE;
+}
+
+bool_t i2c_idle(struct i2c_periph* periph)
+{
+  return PPRZ_I2C_IS_IDLE(periph);
+  //return periph->status == I2CIdle;
+}
+
+
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 1fc2b888c3..d099b0da09 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -55,13 +55,26 @@ void hmc5843_idle_task(void)
 {
   if (hmc5843.timeout > HMC5843_TIMEOUT)
   {
-    hmc5843.timeout = 0;
-    LED_TOGGLE(4);
+//    hmc5843.timeout = 0;
+//    LED_TOGGLE(4);
+  }
+
+  if (i2c_idle(&i2c2))
+  {
+    LED_ON(7);	// green = idle
+    LED_OFF(6);
+  }
+  else
+  {
+    LED_ON(6); // red = busy
+    LED_OFF(7);
   }
 
   // Wait for I2C transaction object to be released by the I2C driver before changing anything
   if ((hmc5843.i2c_trans.status == I2CTransFailed) || (hmc5843.i2c_trans.status == I2CTransSuccess))
   {
+    LED_ON(5);
+    LED_OFF(4);
     if (hmc5843.initialized < 4)
     {
        if (hmc5843.i2c_trans.status == I2CTransSuccess)
@@ -98,6 +111,11 @@ void hmc5843_idle_task(void)
       i2c_submit(&i2c2, &hmc5843.i2c_trans);
     }
   }
+  else
+  {
+    LED_ON(4);
+    LED_OFF(5);
+  }
 
 }
 

From daf6b699341f0c5eb0640cdc6e3c1855a08288d7 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 13 Jul 2011 10:29:57 +0200
Subject: [PATCH 18/74] Separate Reading of SR1 and SR2 to solve race condition
 when reading of the first byte occured before the next start/stop/nack was
 issued: we can now finally read 2 bytes reliably

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 40 +++++++++++++++----
 1 file changed, 32 insertions(+), 8 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 60f70461d9..9e0cd13f88 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -307,10 +307,12 @@ static inline void i2c_event(struct i2c_periph *periph)
   //   reading the status and setting all needed flags
 
   // Direct Access to the I2C Registers
+  static volatile uint8_t stage = 0;
+
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
   volatile uint16_t SR1 = regs->SR1;
-  volatile uint16_t SR2 = regs->SR2;
+  // Do not read SR2 yet as it might start the reading while an (n)ack bit might be needed first
   
   LED1_ON();
   LED1_OFF();
@@ -319,17 +321,39 @@ static inline void i2c_event(struct i2c_periph *periph)
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    regs->DR = 0x3C;
+    regs->DR = 0x3C + stage;
+    stage = 1 - stage;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
-    regs->DR = 0x00;
-    regs->DR = 0x18;
-    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
-  }
-  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
-  {
+    if (stage == 1) // Transmit
+    {
+      // Now read SR2 to clear the ADDR
+      volatile uint16_t SR2 = regs->SR2;
+
+      // Send First 2 bytes
+      regs->DR = 0x00;
+      regs->DR = 0x18;
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+    else // Read Just 1
+    {
+      // First Clear the ACK bit
+      regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+      // Now read SR2 to clear the ADDR
+      volatile uint16_t SR2 = regs->SR2;
+      
+      // Enable the RXNE to get the result
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+      // Program A Stop After
+      regs->CR1 |= I2C_CR1_BIT_STOP;
+
+      // And start again
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
   }
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
   {

From 0805d371b2cf8080b8d787147f19a42df7c93cf2 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 13 Jul 2011 12:13:22 +0200
Subject: [PATCH 19/74] send1 send2 read1

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 202 +++++++++++++-----
 1 file changed, 146 insertions(+), 56 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 9e0cd13f88..f5393f844f 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -101,7 +101,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 30000
+      .I2C_ClockSpeed = 400000
 };
 #endif
 
@@ -227,6 +227,131 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
 
 #define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
 
+// STM32 I2C Transaction Types
+
+enum STMI2CTransactionType {
+  I2CSend1,
+  I2CSend2,
+  I2CSendMany,
+  I2CGet1,
+  I2CGet2,
+  I2CGetMany
+};
+
+enum STMI2CTransactionEndType {
+  STMI2C_StopAndClose,
+  STMI2C_StopAndNewStart,
+  STMI2C_ReStart,
+};
+
+static inline void stmi2c_send1(I2C_TypeDef *regs, uint8_t last_transaction)
+{
+  volatile uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = 0x3C;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // Now read SR2 to clear the ADDR
+    volatile uint16_t SR2 = regs->SR2;
+    if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) {}
+
+    // Send Bytes
+    regs->DR = 0x03;
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+
+    // BTF is set as soon as the shift register is empty. 
+    // BTF is cleared A) when writing data to DR or B) when a start/stop condition OCCURRED (not was requested)
+    // Dummy Data to avoid BTF
+    regs->DR = 0x00;
+
+    // After the stop: start again
+    if (! last_transaction )
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
+  }
+}
+
+static inline void stmi2c_send2(I2C_TypeDef *regs, uint8_t last_transaction)
+{
+  volatile uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = 0x3C;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // Now read SR2 to clear the ADDR
+    volatile uint16_t SR2 = regs->SR2;
+
+    if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2))
+    {
+    }
+
+
+    // Send First 2 bytes
+    regs->DR = 0x00;
+    regs->DR = 0x18;
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
+  {
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Also provide some dummy data in DR to silent the BTF interrupt
+    regs->DR = 0x00;
+
+    // After the stop: start again
+    if (! last_transaction )
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
+  }
+}
+
+static inline void stmi2c_read1(I2C_TypeDef *regs, uint8_t last_transaction)
+{
+  volatile uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = 0x3C + 1;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // First Clear the ACK bit
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    volatile uint16_t SR2 = regs->SR2;
+      
+    // Enable the RXNE to get the result
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    // Program A Stop After
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+
+    // And start again
+    if (! last_transaction )
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
+  }
+}
+
 static inline void i2c_event(struct i2c_periph *periph)
 {
   /*	
@@ -244,19 +369,27 @@ static inline void i2c_event(struct i2c_periph *periph)
 	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
 
 	// Beware: using the buffered I2C has some interesting properties:
-	  -when receiving BTF only occurs after the 2nd received byte: after the first byte is received it is 
+	  -in receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
            in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
 	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost)
-	  -when transmitting, and writing a byte to WD, you instantly get a new TxE interrupt while the first is not
-	   transmitted yet. The byte was pushed to the I2C serializer and the buffer is ready for more. You can already
+	  -in transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
 	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+        
+        // Beware: besides buffering there is also event sheduling. You can send 2 bytes to the buffer, ask for a stop and 
+           a new start in one go. 
+
+          -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
+           This also means you must think more in advance and a transaction could be popped from the stack even before it is
+           actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
+           is known.
 	    
-	// Beware: the order in which Status is read determines how flags are cleared.
+	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
 
 	If IT_EV_FEN AND IT_EV_BUF
 	--------------------------
 
-	We are always interested in buffer interrupts IT_EV_BUF except in transmission when all data was sent
+	Buffer event are not always wanted and are tipically switched on during longer data transfers. It highly depends on the data size.
 
 	6) RxNE
 	7) TxE
@@ -264,7 +397,8 @@ static inline void i2c_event(struct i2c_periph *periph)
 	--------------------------------------------------------------------------------------------------
 	// This driver uses only a subset of the pprz_i2c_states for several reasons:
 	// -we have less interrupts than the I2CStatus states (for efficiency)
-	// -status register flags better correspond to reality, especially in case of I2C errors
+	// -STM32 has such a powerfull I2C engine with plenty of status register flags that
+            only little extra status information needs to be stored. 
 
 	enum I2CStatus {
 	  I2CIdle,			// Dummy: Actual I2C Peripheral idle detection is safer with the 
@@ -287,7 +421,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
 	---------
 
-	The STM waits (holding SCL low) for user interaction:
+	The STM waits indefinately (holding SCL low) for user interaction:
 	a) after a master-start (waiting for address)
 	b) after an address (waiting for data)
 	   not during data sending when using buffered
@@ -295,6 +429,9 @@ static inline void i2c_event(struct i2c_periph *periph)
 	   not during data receiving when using buffered
 	   not after the last byte is received
 
+	The STM I2C stalls indefinately when a stop condition was attempted that
+	did not succeed. The BUSY flag remains on
+
    */
 
 
@@ -311,60 +448,13 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
-  volatile uint16_t SR1 = regs->SR1;
   // Do not read SR2 yet as it might start the reading while an (n)ack bit might be needed first
   
   LED1_ON();
   LED1_OFF();
 
-  // Start Condition Was Just Generated
-  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
-  {
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    regs->DR = 0x3C + stage;
-    stage = 1 - stage;
-  }
-  // Address Was Sent
-  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
-  {
-    if (stage == 1) // Transmit
-    {
-      // Now read SR2 to clear the ADDR
-      volatile uint16_t SR2 = regs->SR2;
+  stmi2c_read1(regs, FALSE);
 
-      // Send First 2 bytes
-      regs->DR = 0x00;
-      regs->DR = 0x18;
-      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
-    }
-    else // Read Just 1
-    {
-      // First Clear the ACK bit
-      regs->CR1 &= ~ I2C_CR1_BIT_ACK;
-
-      // Now read SR2 to clear the ADDR
-      volatile uint16_t SR2 = regs->SR2;
-      
-      // Enable the RXNE to get the result
-      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-
-      // Program A Stop After
-      regs->CR1 |= I2C_CR1_BIT_STOP;
-
-      // And start again
-      regs->CR1 |= I2C_CR1_BIT_START;
-    }
-  }
-  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
-  {
-    regs->CR1 |= I2C_CR1_BIT_STOP;
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    // Also provide some dummy data in DR to silent the BTF interrupt
-    regs->DR = 0x00;
-
-    // After the stop: start again
-    regs->CR1 |= I2C_CR1_BIT_START;
-  }
 
   return;
 

From 0780d24cba592e5950bffb13d8f1315c75975afd Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 13 Jul 2011 23:23:22 +0200
Subject: [PATCH 20/74] send1 send2 send-many read1 read2 + transaction
 sheduler + err AF. Todo berr/arlo/...

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 591 ++++++++++--------
 sw/airborne/peripherals/hmc5843.c             |  10 +-
 2 files changed, 321 insertions(+), 280 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index f5393f844f..f92867b72e 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -8,6 +8,7 @@
 //#include "led.h" 
 
 /////////// DEBUGGING //////////////
+// TODO: remove this 
 
 static inline void LED1_ON(void)
 {
@@ -57,32 +58,9 @@ static inline void LED_ERROR(uint8_t nr)
   }
 }
 
-static inline void LED_STROBE2(void)
-{
-LED2_ON();
-LED2_OFF();
-LED1_ON();
-LED1_OFF();
-LED2_ON();
-LED2_OFF();
-LED1_ON();
-LED1_OFF();
-LED1_OFF();
-LED1_OFF();
-LED1_OFF();
-
-}
 
 //////////////////////////////////////
 
-#ifdef DEBUG_I2C
-#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); }
-#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); }
-#else
-#define SPURIOUS_INTERRUPT(_periph, _status, _event) { }
-#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { }
-#endif
-
 #ifdef USE_I2C1
 static I2C_InitTypeDef  I2C1_InitStruct = {
       .I2C_Mode = I2C_Mode_I2C,
@@ -126,68 +104,23 @@ static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* periph)
 static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
   periph->idx_buf = 0;
+  periph->status = I2CStartRequested;
+  // After the stop: start again
+  // regs->CR1 |= I2C_CR1_BIT_START;
   I2C_GenerateSTART(periph->reg_addr, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
 }
 
-static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph) 
-{
-  /* if we have no more transaction to process, stop here */
-  if (periph->trans_extract_idx == periph->trans_insert_idx)
-  {
-    // Should we disable just in case? normally not. So if more interrupts are 
-    // triggered there is a problem and we want to know.
-    // I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
-    periph->status = I2CIdle;
-  }
-  /* if not, start next transaction */
-  else
-  {
-    periph->status = I2CStartRequested;
-    PPRZ_I2C_SEND_START(periph);
-  }
-}
-
-static inline void PPRZ_I2C_RESTART(struct i2c_periph *periph)
-{
-//LED2_ON();
-//        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-//      I2C_SendData(periph->reg_addr, 0);
-//  I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BTF);
-  periph->status = I2CRestartRequested;
-  PPRZ_I2C_SEND_START(periph);
-}
-
-// STOP
-
-static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_transaction *trans, enum I2CTransactionStatus _status)
-{
-  // Finish Current
-  trans->status = _status;
- 
-  // When finished successfully the I2C_FLAG_MLS will be cleared after the stop condition was issued.
-  // However: we do not need to wait for it to go the the next step, but if no stop condition was 
-  // sent yet than we are still talking to the same slave...
-  // When we are here all paths to this function with success have already issued a STOP, the others not.
-  // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
-  if (_status != I2CTransSuccess)
-  {
-    // TODO: we might need to do much more here: see reset functions of antoine...
-    I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-  }  
-
-  // Jump to the next
-  periph->trans_extract_idx++;
-  if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
-    periph->trans_extract_idx = 0;
-
-  PPRZ_I2C_START_NEXT_TRANSACTION(periph);
-}
-
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
+// Bypassing the libSTM I2C functions to have more control over the reading of registers
+// e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
+
+// Referring to STM32 manual: 
+// -Doc ID 13902 Rev 11
+
 // Status Register 1
 
 #define I2C_SR1_BIT_SB			(1<<0)		// Start Condition Met
@@ -197,9 +130,10 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
 #define I2C_SR1_BIT_TXE			(1<<7)		// TX buffer space available
 
 #define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop
+#define I2C_SR1_BIT_ERR_ARLO		(1<<9)		// Misplaced Start/Stop
 #define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure
 
-#define I2C_SR1_BITS_ERR		(I2C_SR_BIT_ERR_BUS|I2C_SR_BIT_ERR_AF)
+#define I2C_SR1_BITS_ERR		((1<<8)|(1<<9)|(1<<10)|(1<<11)|(1<<12)|(1<<14)|(1<<15))
 
 // Status Register 2
 
@@ -227,107 +161,101 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
 
 #define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
 
-// STM32 I2C Transaction Types
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+//	SUBTRANSACTION SEQUENCES
 
-enum STMI2CTransactionType {
-  I2CSend1,
-  I2CSend2,
-  I2CSendMany,
-  I2CGet1,
-  I2CGet2,
-  I2CGetMany
+enum STMI2CSubTransactionStatus {
+  STMI2C_SubTra_Busy,
+  STMI2C_SubTra_Ready_StopRequested,
+  STMI2C_SubTra_Ready
 };
 
-enum STMI2CTransactionEndType {
-  STMI2C_StopAndClose,
-  STMI2C_StopAndNewStart,
-  STMI2C_ReStart,
-};
-
-static inline void stmi2c_send1(I2C_TypeDef *regs, uint8_t last_transaction)
+static inline enum STMI2CSubTransactionStatus stmi2c_send1(I2C_TypeDef *regs, struct i2c_transaction *trans)
 {
-  volatile uint16_t SR1 = regs->SR1;
+  uint16_t SR1 = regs->SR1;
 
   // Start Condition Was Just Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    regs->DR = 0x3C;
+    regs->DR = trans->slave_addr;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // Now read SR2 to clear the ADDR
-    volatile uint16_t SR2 = regs->SR2;
-    if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) {}
+    volatile uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
 
     // Send Bytes
-    regs->DR = 0x03;
-    regs->CR1 |= I2C_CR1_BIT_STOP;
+    regs->DR = trans->buf[0];
 
     // BTF is set as soon as the shift register is empty. 
     // BTF is cleared A) when writing data to DR or B) when a start/stop condition OCCURRED (not was requested)
     // Dummy Data to avoid BTF
     regs->DR = 0x00;
 
-    // After the stop: start again
-    if (! last_transaction )
-    {
-      regs->CR1 |= I2C_CR1_BIT_START;
-    }
+    return STMI2C_SubTra_Ready;
   }
+
+  return STMI2C_SubTra_Busy;
 }
 
-static inline void stmi2c_send2(I2C_TypeDef *regs, uint8_t last_transaction)
+static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
-  volatile uint16_t SR1 = regs->SR1;
+  uint16_t SR1 = regs->SR1;
 
   // Start Condition Was Just Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    regs->DR = 0x3C;
+    regs->DR = trans->slave_addr;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // Now read SR2 to clear the ADDR
-    volatile uint16_t SR2 = regs->SR2;
-
-    if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2))
-    {
-    }
+    uint16_t SR2  __attribute__ ((unused)) = regs->SR2;
 
+    if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
 
     // Send First 2 bytes
-    regs->DR = 0x00;
-    regs->DR = 0x18;
+    regs->DR = trans->buf[0];
+    regs->DR = trans->buf[1];
+    periph->idx_buf = 2;
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
   {
-    regs->CR1 |= I2C_CR1_BIT_STOP;
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    // Also provide some dummy data in DR to silent the BTF interrupt
-    regs->DR = 0x00;
-
-    // After the stop: start again
-    if (! last_transaction )
+    // All bytes Sent?
+    if ( periph->idx_buf >= trans->len_w)
     {
-      regs->CR1 |= I2C_CR1_BIT_START;
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+      // Also provide some dummy data in DR to silent the BTF interrupt
+      regs->DR = 0x00;
+
+      return STMI2C_SubTra_Ready;
+    }
+    else
+    {
+      regs->DR = trans->buf[periph->idx_buf];
+      periph->idx_buf++;
     }
   }
+
+  return STMI2C_SubTra_Busy;
 }
 
-static inline void stmi2c_read1(I2C_TypeDef *regs, uint8_t last_transaction)
+static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, struct i2c_transaction *trans)
 {
-  volatile uint16_t SR1 = regs->SR1;
+  uint16_t SR1 = regs->SR1;
 
   // Start Condition Was Just Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    regs->DR = 0x3C + 1;
+    regs->DR = trans->slave_addr | 0x01;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
@@ -336,24 +264,86 @@ static inline void stmi2c_read1(I2C_TypeDef *regs, uint8_t last_transaction)
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
-    volatile uint16_t SR2 = regs->SR2;
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
-    // Enable the RXNE to get the result
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-
-    // Program A Stop After
+    // Schedule a Stop
+        LED2_ON();
+	LED2_OFF();
     regs->CR1 |= I2C_CR1_BIT_STOP;
 
-    // And start again
-    if (! last_transaction )
-    {
-      regs->CR1 |= I2C_CR1_BIT_START;
-    }
+    // Enable the RXNE to get the result
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    trans->buf[0] = regs->DR;
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+
+  return STMI2C_SubTra_Busy;
 }
 
+static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    regs->CR1 |= I2C_CR1_BIT_POS;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+      
+    // NOT First Clear the ACK bit but only AFTER clearing ADDR
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Disable the RXNE and wait for BTF
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Stop condition MUST be set BEFORE reading the DR
+    // otherwise since there is new buffer space a new byte will be read
+        LED2_ON();
+	LED2_OFF();
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+
+    trans->buf[0] = regs->DR;
+    trans->buf[1] = regs->DR;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_handle_errors(I2C_TypeDef *regs, struct i2c_periph *periph)
+{
+  return STMI2C_SubTra_Busy;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+//	TRANSACTION HANDLER
+
+static inline uint8_t STMI2CTansactionHandler(struct i2c_periph *periph)
+{
+  return 0;
+}
+
+
 static inline void i2c_event(struct i2c_periph *periph)
 {
+
   /*	
 	There are 7 possible reasons to get here:
 
@@ -398,24 +388,26 @@ static inline void i2c_event(struct i2c_periph *periph)
 	// This driver uses only a subset of the pprz_i2c_states for several reasons:
 	// -we have less interrupts than the I2CStatus states (for efficiency)
 	// -STM32 has such a powerfull I2C engine with plenty of status register flags that
-            only little extra status information needs to be stored. 
+            only little extra status information needs to be stored.
+
+       // Status is re-used (abused) to remember the last COMMAND THAT WAS SENT to the STM I2C hardware.
 
 	enum I2CStatus {
-	  I2CIdle,			// Dummy: Actual I2C Peripheral idle detection is safer with the 
-					// hardware status register flag I2C_FLAG_BUSY.
+	  I2CIdle,			// No more last command
 
-	  I2CStartRequested,		// EV5: used to differentiate S en Sr
-	  I2CRestartRequested,		// EV5: used to differentiate S en Sr
+	  I2CStartRequested,		// Last command was start
+	  I2CRestartRequested,		// Last command was restart
+	  I2CStopRequested,		// Very important to not send double stop conditions
 
-	  I2CSendingByte,		// Not used: using the hardware status reg I2C_FLAG_TRA
+	  I2CSendingByte,		// Some address/data operation
+
+	  // Following are not used
 	  I2CReadingByte,
-	  I2CAddrWrSent,		// Since we can do many things at once and we
-	  I2CAddrRdSent,		// have buffered sending, these states
-	  I2CSendingLastByte,		// do not correspond to the real state of the
-	  I2CReadingLastByte,		// STM I2C driver so they are not used
-	  I2CStopRequested,		
-
-	  I2CComplete,			// Used to provide the result
+	  I2CAddrWrSent,	
+	  I2CAddrRdSent,
+	  I2CSendingLastByte,
+	  I2CReadingLastByte,
+	  I2CComplete,
 	  I2CFailed
 	};
 
@@ -444,23 +436,183 @@ static inline void i2c_event(struct i2c_periph *periph)
   //   reading the status and setting all needed flags
 
   // Direct Access to the I2C Registers
-  static volatile uint8_t stage = 0;
-
+  // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-
-  // Do not read SR2 yet as it might start the reading while an (n)ack bit might be needed first
   
   LED1_ON();
   LED1_OFF();
 
-  stmi2c_read1(regs, FALSE);
+  if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_AF ))
+  {
+    // Disable Buffer interrupts
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Silent any BTF
+    regs->DR = 0x00; 
+
+    // Unless one is already scheduled: give a stop
+    if (periph->status != I2CStopRequested)
+    {
+      regs->CR1 |= I2C_CR1_BIT_STOP;
+    }
+    
+    // Followed by a fresh Start
+    regs->CR1 |= I2C_CR1_BIT_START;
+  }
+  else if (regs->SR1 & I2C_SR1_BITS_ERR)
+  {
+    // Disable Buffer interrupts
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    // If we became slave (e.g. ARLO, etc ...)
+    if (! BIT_X_IS_SET_IN_REG( regs->SR2 , I2C_SR2_BIT_MSL ))
+    {
+      // Become master again
+      regs->CR1 |= I2C_CR1_BIT_START;
+      return;
+    }
+
+    // If arbitration was lost (can only happen with many master / interference / SDA short)
+    // then STM I2C is back to slave mode. Issue a New Start right away as we are the only
+    // master to try again
+    if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_ARLO ))
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+      return;
+    }
+
+
+    // Shedule a Stop Condition, unless one was already sheduled: 
+    // then we would get a double stop and no interrupts
+    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_STOP ))
+    {
+        LED2_ON();
+	LED2_OFF();
+      regs->CR1 |= I2C_CR1_BIT_STOP;
+    }
+    // Silent BTF Interrupts and shedule a byte of no bytes were left
+    regs->DR = 0x00;
+
+    // If not last: New Start
+    // If start already asked, then do nothing but wait for its interrupt
+    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_START ))
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
+
+    // Reset WatchDog
+
+//    stage = 1 - stage;
+    return;
+  }
+
+  enum STMI2CSubTransactionStatus ret = 0;
+  uint8_t restart = 0;
+
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    // no transaction?
+    periph->status = I2CIdle;
+    periph->errors->unexpected_event_cnt++;
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+
+  if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
+  {
+    switch (trans->len_r)
+    {
+      case 1:
+        ret = stmi2c_read1(regs,trans);
+        restart = 1;
+        break;
+      case 2:
+        ret = stmi2c_read2(regs,trans);
+        break;
+      default:
+        ret = stmi2c_read2(regs,trans);
+        break;    
+    }
+  }
+  else // TxRx or Tx
+  {
+    if (trans->len_w > 1)
+    {
+      ret = stmi2c_sendmany(regs,periph,trans);
+    }    
+    else
+    {
+      ret = stmi2c_send1(regs,trans);
+    }
+    if (trans->type == I2CTransTxRx)
+    {
+      restart = 1;
+    }
+  }
+
+  // Sub-transaction not finished
+  if (ret == STMI2C_SubTra_Busy)
+  {
+    // Remember the last command was not start or stop
+    periph->status = I2CSendingByte;
+  }
+  else // Finished?
+  {
+    if (restart == 0)
+    {
+      // Finish Current
+      trans->status = I2CTransSuccess;
+
+      if (ret == STMI2C_SubTra_Ready)
+      {
+        LED2_ON();
+	LED2_OFF();
+        // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+        regs->CR1 |= I2C_CR1_BIT_STOP;
+
+        periph->status = I2CStopRequested;
+      }
+
+      // Jump to the next transaction
+      periph->trans_extract_idx++;
+      if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+        periph->trans_extract_idx = 0;
+
+      // if we have no more transaction to process, stop here 
+      if (periph->trans_extract_idx == periph->trans_insert_idx)
+      {
+        periph->status = I2CIdle;
+      }
+      // if not, start next transaction
+      else
+      {
+        // Restart transaction doing the Rx part now
+        periph->status = I2CStartRequested;
+        PPRZ_I2C_SEND_START(periph);
+     }
+
+    }
+    // RxTx -> Restart and do Rx part
+    else 
+    {
+      trans->type = I2CTransRx;
+      periph->status = I2CStartRequested;
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
+  }
+
+
+
+
+
+
+
+
 
 
   return;
 
-  // Referring to manual: 
-  // -Doc ID 13902 Rev 11
-
+/*
 
   // Check to make sure that user space has an active transaction pending
   if (periph->trans_extract_idx == periph->trans_insert_idx)
@@ -472,28 +624,6 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
 
-
-  LED1_ON();
-
-  //I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-
-  //__disable_irq();
-  uint32_t event = I2C_GetLastEvent(periph->reg_addr);
-
-  //uint32_t event = (((uint32_t)(regs->SR2)) << 16) + regs->SR1;
-
-  if (event & I2C_FLAG_TXE)
-  {
-    LED2_ON();
-  }
-
-
-  ///////////////////////////////////////////////////////////////////////////////////
-  ///////////////////////////////////////////////////////////////////////////////////
-  // START: Start Condition in Master Mode: 
-  // STM Manual Ev5
-  if (event & I2C_FLAG_SB)
-  {
     // Periph was waiting for Start
     if (periph->status == I2CStartRequested)
     {
@@ -524,105 +654,10 @@ static inline void i2c_event(struct i2c_periph *periph)
     periph->status = I2CAddrWrSent;
   }
 
-  ///////////////////////////////////////////////////////////////////////////////////
-  ///////////////////////////////////////////////////////////////////////////////////
-  // TRANSMIT: Buffer Can accept the next byte for transmission
-  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt 
-  //     will be triggered until a start/stop occurs which can be quite long = many spurrious interrupts)
-  // STM Manual Ev8
-  else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and no flag tra/start/stop/addr
-  {
-    if (periph->status == I2CRestartRequested)
-    {
-      // Neglect this interrupt: We just issued the restart last session already
-          PPRZ_I2C_RESTART(periph);
-    }
-
-    // Do we have more data? and there is buffer room? 
-    // (neglect BTF: if it was set it just means we were too slow)
-    else if (periph->idx_buf < trans->len_w)
-    {
-      I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
-      periph->idx_buf++;
-      // Was this one the Last? -> Disable the buf interrupt (until next start) and wait for BTF
-      // -we could gain a bit of efficiency by already starting the next action but for 
-      //  code-readability we will wait until the last tx-byte is sent
-      if (periph->idx_buf >= trans->len_w)
-      {
-        I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
-        // If this is followed by a restart: then we need to set the startbit to avoid extra interrupts.
-        if (trans->type != I2CTransTx) 
-        {
-        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-        }
-      } 
-    }
-
-    else if (event & I2C_FLAG_BTF)
-    {
-      // Ready -> Stop
-      if (trans->type == I2CTransTx) 
-      {
-        // STM Manual Ev8_2 
-        I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-        PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
-      }
-      // Rx/Trans -> Restart: 
-      // Do not wait for BTF
-    }
-    // If we had no more data but got no BTF then there is a problem
-    else
-    {
-      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
-    }
-  }
-
-  ///////////////////////////////////////////////////////////////////////////////////
-  ///////////////////////////////////////////////////////////////////////////////////
-  // RECEIVE:
-  // while receiving: the master needs to signal to the slave if more data is needed
-  else if ((event & I2C_FLAG_ADDR) || (event & I2C_FLAG_RXNE))
-  {
     // data is available every time RXNE is set. If BTF is set it means that 2 bytes are 
     // ready to read (one in shift register) and I2C has stopped until the buffer can accept new data.
-    if (event & I2C_FLAG_RXNE) 
-    {
-      uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-      if (periph->idx_buf < trans->len_r) 
-      {
-        trans->buf[periph->idx_buf] = read_byte;
-        periph->idx_buf++;
-      }
-    }
-
-    // This last byte has arrived
-    if (periph->idx_buf >= trans->len_r) 
-    {
-      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
-    }
-    // Tell the Slave it will be the last one
-    else if (periph->idx_buf >= trans->len_r-1) 
-    {
-      I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);         // give them a nack once it's done
-      I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and follow with a stop
-    }
-    // Ask the Slave to send more
-    else
-    {
-      I2C_AcknowledgeConfig(periph->reg_addr, ENABLE);
-    }
-
-  }
-
-  // Now re-enable IRQ... it's been too long  
-  // __enable_irq();
-
-
-
-  LED2_OFF();
-  LED1_OFF();
-
 
+*/
 }
 
 static inline void i2c_error(struct i2c_periph *periph)
@@ -668,9 +703,12 @@ static inline void i2c_error(struct i2c_periph *periph)
     err_nr = 7;
   }
 
-
   LED_ERROR(err_nr);
 
+  i2c_event(periph);
+  return;
+  
+/*
 
   // Check to make sure that user space has an active transaction pending
   if (periph->trans_extract_idx == periph->trans_insert_idx)
@@ -688,6 +726,7 @@ static inline void i2c_error(struct i2c_periph *periph)
 
   PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
 
+*/
 
 }
 
@@ -834,7 +873,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
   /* if peripheral is idle, start the transaction */
   // if (PPRZ_I2C_IS_IDLE(p))
   if (periph->status == I2CIdle)
-    PPRZ_I2C_START_NEXT_TRANSACTION(periph);
+    PPRZ_I2C_SEND_START(periph);
   /* else it will be started by the interrupt handler when the previous transactions completes */
   __enable_irq();
 
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index d099b0da09..f4ad281a3f 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -25,11 +25,13 @@ static void hmc_send_config(uint8_t _init)
     hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
     hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
-    hmc5843.i2c_trans.len_w = 2;
+    hmc5843.i2c_trans.buf[2] = 0x01<<5;
+    hmc5843.i2c_trans.buf[3] = 0x00;
+    hmc5843.i2c_trans.len_w = 4;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
     break;
   case 2:
-    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.type = I2CTransRx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
     hmc5843.i2c_trans.buf[1] = 0x01<<5;
     hmc5843.i2c_trans.len_w = 2;
@@ -39,12 +41,12 @@ static void hmc_send_config(uint8_t _init)
     hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
     hmc5843.i2c_trans.buf[1] = 0x00;
-    hmc5843.i2c_trans.len_w = 2;
+    hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   default: 
     hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 6;
+    hmc5843.i2c_trans.len_r = 2;
     hmc5843.i2c_trans.len_w = 1;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
     i2c_submit(&i2c2, &hmc5843.i2c_trans);

From 6542854ba97065ea4a10db6dabbb02705d5b1235 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 14 Jul 2011 13:18:42 +0200
Subject: [PATCH 21/74] test software using all transaction types

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c    |  8 ++++----
 sw/airborne/peripherals/hmc5843.c                 | 15 ++++++++++++---
 2 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index f92867b72e..a058934711 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -129,9 +129,10 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 #define I2C_SR1_BIT_RXNE		(1<<6)		// Data Read Available
 #define I2C_SR1_BIT_TXE			(1<<7)		// TX buffer space available
 
-#define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop
-#define I2C_SR1_BIT_ERR_ARLO		(1<<9)		// Misplaced Start/Stop
-#define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure
+#define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop (usually interference)
+#define I2C_SR1_BIT_ERR_ARLO		(1<<9)		// Arbitration Lost (in multimaster) or SDA short-to-ground (in single master)
+#define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure (too fast/too soon/no sensor/wiring break/...)
+#define I2C_SR1_BIT_ERR_OVR		(1<<11)		// Overrun [data loss] (in slave) or SCL short-to-ground (in single master)
 
 #define I2C_SR1_BITS_ERR		((1<<8)|(1<<9)|(1<<10)|(1<<11)|(1<<12)|(1<<14)|(1<<15))
 
@@ -524,7 +525,6 @@ static inline void i2c_event(struct i2c_periph *periph)
     {
       case 1:
         ret = stmi2c_read1(regs,trans);
-        restart = 1;
         break;
       case 2:
         ret = stmi2c_read2(regs,trans);
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index f4ad281a3f..ee8ce3db13 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -39,11 +39,20 @@ static void hmc_send_config(uint8_t _init)
   break;
   case 3:
     hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
-    hmc5843.i2c_trans.buf[1] = 0x00;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to continuous mode
     hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
+  case 4:
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 1;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  case 5:
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 2;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
   default: 
     hmc5843.i2c_trans.type = I2CTransTxRx;
     hmc5843.i2c_trans.len_r = 2;
@@ -77,7 +86,7 @@ void hmc5843_idle_task(void)
   {
     LED_ON(5);
     LED_OFF(4);
-    if (hmc5843.initialized < 4)
+    if (hmc5843.initialized < 6)
     {
        if (hmc5843.i2c_trans.status == I2CTransSuccess)
        {

From 3df0f1eca0d8e89c4a2d47fd088fb57ff27bddba Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 14 Jul 2011 17:05:44 +0200
Subject: [PATCH 22/74] I2C Bus error investigation

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 278 +++++++++++-------
 sw/airborne/peripherals/hmc5843.c             |  20 +-
 2 files changed, 186 insertions(+), 112 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index a058934711..5f505830aa 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -46,16 +46,16 @@ static inline void LED_INIT(void)
 
 static inline void LED_ERROR(uint8_t nr)
 {
+  LED2_ON();
   for (int i=0;i<20;i++)
   {
-    LED1_ON();
-    LED1_OFF();
     if (nr == i)
-      LED2_OFF();
+      LED1_OFF();
     else
-      LED2_ON();
-    LED2_OFF();    
+      LED1_ON();
+    LED1_OFF();    
   }
+  LED2_OFF();
 }
 
 
@@ -105,8 +105,9 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
   periph->idx_buf = 0;
   periph->status = I2CStartRequested;
-  // After the stop: start again
-  // regs->CR1 |= I2C_CR1_BIT_START;
+  // Clear any pending stop
+  I2C_GenerateSTOP(periph->reg_addr, DISABLE);
+  // Issue a new start
   I2C_GenerateSTART(periph->reg_addr, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
@@ -197,6 +198,12 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send1(I2C_TypeDef *regs, st
     // Dummy Data to avoid BTF
     regs->DR = 0x00;
 
+    if (trans->type == I2CTransTx)
+    {
+      // We sent it all to the I2C ... might still be a chance that an error occurs
+      trans->status = I2CTransSuccess;
+    }
+
     return STMI2C_SubTra_Ready;
   }
 
@@ -236,6 +243,12 @@ static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs,
       // Also provide some dummy data in DR to silent the BTF interrupt
       regs->DR = 0x00;
 
+      if (trans->type == I2CTransTx)
+      {
+        // We sent it all to the I2C ... might still be a chance that an error occurs
+        trans->status = I2CTransSuccess;
+      }
+
       return STMI2C_SubTra_Ready;
     }
     else
@@ -279,6 +292,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     trans->buf[0] = regs->DR;
+
+    // We got all the results
+    trans->status = I2CTransSuccess;
+
     return STMI2C_SubTra_Ready_StopRequested;
   }
 
@@ -320,27 +337,88 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     trans->buf[0] = regs->DR;
     trans->buf[1] = regs->DR;
 
+    // We got all the results
+    trans->status = I2CTransSuccess;
+
     return STMI2C_SubTra_Ready_StopRequested;
   }
 
   return STMI2C_SubTra_Busy;
 }
 
-static inline enum STMI2CSubTransactionStatus stmi2c_handle_errors(I2C_TypeDef *regs, struct i2c_periph *periph)
+/*
+static inline enum STMI2CSubTransactionStatus stmi2c_handle_errors(I2C_TypeDef *regs, struct i2c_transaction *trans, struct i2c_periph *periph)
 {
+  // Sensor did not Acknowlegde
+  if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_AF ))
+  {
+    // Disable Buffer interrupts
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Silent any BTF
+    regs->DR = 0x00; 
+
+    return STM_I2C_SubTra_Ready;
+
+    // Unless one is already scheduled: give a stop
+    if (periph->status != I2CStopRequested)
+    {
+      regs->CR1 |= I2C_CR1_BIT_STOP;
+    }
+    
+    // Followed by a fresh Start
+    regs->CR1 |= I2C_CR1_BIT_START;
+  }
+  else if (regs->SR1 & I2C_SR1_BITS_ERR)
+  {
+    // Disable Buffer interrupts
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    // If we became slave (e.g. ARLO, etc ...)
+    if (! BIT_X_IS_SET_IN_REG( regs->SR2 , I2C_SR2_BIT_MSL ))
+    {
+      // Become master again
+      regs->CR1 |= I2C_CR1_BIT_START;
+      return;
+    }
+
+    // If arbitration was lost (can only happen with many master / interference / SDA short)
+    // then STM I2C is back to slave mode. Issue a New Start right away as we are the only
+    // master to try again
+    if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_ARLO ))
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+      return;
+    }
+
+
+    // Shedule a Stop Condition, unless one was already sheduled: 
+    // then we would get a double stop and no interrupts
+    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_STOP ))
+    {
+        LED2_ON();
+	LED2_OFF();
+      regs->CR1 |= I2C_CR1_BIT_STOP;
+    }
+    // Silent BTF Interrupts and shedule a byte of no bytes were left
+    regs->DR = 0x00;
+
+    // If not last: New Start
+    // If start already asked, then do nothing but wait for its interrupt
+    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_START ))
+    {
+      regs->CR1 |= I2C_CR1_BIT_START;
+    }
+
+    // Reset WatchDog
+
+//    stage = 1 - stage;
+    return;
+  }
   return STMI2C_SubTra_Busy;
 }
+*/
 
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-//
-//	TRANSACTION HANDLER
-
-static inline uint8_t STMI2CTansactionHandler(struct i2c_periph *periph)
-{
-  return 0;
-}
-
+static inline void i2c_error(struct i2c_periph *periph);
 
 static inline void i2c_event(struct i2c_periph *periph)
 {
@@ -443,110 +521,97 @@ static inline void i2c_event(struct i2c_periph *periph)
   LED1_ON();
   LED1_OFF();
 
-  if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_AF ))
+/*
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_SB, regs->SR1) )
   {
-    // Disable Buffer interrupts
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    // Silent any BTF
-    regs->DR = 0x00; 
-
-    // Unless one is already scheduled: give a stop
-    if (periph->status != I2CStopRequested)
-    {
-      regs->CR1 |= I2C_CR1_BIT_STOP;
-    }
-    
-    // Followed by a fresh Start
-    regs->CR1 |= I2C_CR1_BIT_START;
+    LED2_ON();
+    LED2_OFF();
   }
-  else if (regs->SR1 & I2C_SR1_BITS_ERR)
-  {
-    // Disable Buffer interrupts
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+*/
 
-    // If we became slave (e.g. ARLO, etc ...)
-    if (! BIT_X_IS_SET_IN_REG( regs->SR2 , I2C_SR2_BIT_MSL ))
-    {
-      // Become master again
-      regs->CR1 |= I2C_CR1_BIT_START;
-      return;
-    }
-
-    // If arbitration was lost (can only happen with many master / interference / SDA short)
-    // then STM I2C is back to slave mode. Issue a New Start right away as we are the only
-    // master to try again
-    if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_ARLO ))
-    {
-      regs->CR1 |= I2C_CR1_BIT_START;
-      return;
-    }
-
-
-    // Shedule a Stop Condition, unless one was already sheduled: 
-    // then we would get a double stop and no interrupts
-    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_STOP ))
-    {
-        LED2_ON();
-	LED2_OFF();
-      regs->CR1 |= I2C_CR1_BIT_STOP;
-    }
-    // Silent BTF Interrupts and shedule a byte of no bytes were left
-    regs->DR = 0x00;
-
-    // If not last: New Start
-    // If start already asked, then do nothing but wait for its interrupt
-    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_START ))
-    {
-      regs->CR1 |= I2C_CR1_BIT_START;
-    }
-
-    // Reset WatchDog
-
-//    stage = 1 - stage;
-    return;
-  }
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  //
+  //	TRANSACTION HANDLER
 
   enum STMI2CSubTransactionStatus ret = 0;
   uint8_t restart = 0;
 
+  // Nothing Left To Do
   if (periph->trans_extract_idx == periph->trans_insert_idx)
   {
-    // no transaction?
     periph->status = I2CIdle;
     periph->errors->unexpected_event_cnt++;
+
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+    regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
+    regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
+
     return;
   }
 
   struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
 
-  if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
+  if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
   {
-    switch (trans->len_r)
-    {
-      case 1:
-        ret = stmi2c_read1(regs,trans);
-        break;
-      case 2:
-        ret = stmi2c_read2(regs,trans);
-        break;
-      default:
-        ret = stmi2c_read2(regs,trans);
-        break;    
-    }
+    // Set result in transaction
+    trans->status = I2CTransFailed;
+
+    // Close the Bus
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
+    regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
+        LED1_ON();
+        LED2_ON();
+	LED2_OFF();
+	LED1_OFF();
+    periph->status = I2CStopRequested;
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
+    regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
+
+    // Prepare for next
+    ret = STMI2C_SubTra_Ready_StopRequested;
+    restart = 0;
+
+    // Count it
+    i2c_error(periph);
   }
-  else // TxRx or Tx
+  else
   {
-    if (trans->len_w > 1)
+
+    if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
     {
-      ret = stmi2c_sendmany(regs,periph,trans);
-    }    
-    else
-    {
-      ret = stmi2c_send1(regs,trans);
+      switch (trans->len_r)
+      {
+        case 1:
+          ret = stmi2c_read1(regs,trans);
+          break;
+        case 2:
+          ret = stmi2c_read2(regs,trans);
+          break;
+        default:
+          ret = stmi2c_read2(regs,trans);
+          break;    
+      }
     }
-    if (trans->type == I2CTransTxRx)
+    else // TxRx or Tx
     {
-      restart = 1;
+      if (trans->len_w > 1)
+      {
+        ret = stmi2c_sendmany(regs,periph,trans);
+      }    
+      else
+      {
+        ret = stmi2c_send1(regs,trans);
+      }
+      if (trans->type == I2CTransTxRx)
+      {
+        restart = 1;
+      }
     }
   }
 
@@ -560,13 +625,12 @@ static inline void i2c_event(struct i2c_periph *periph)
   {
     if (restart == 0)
     {
-      // Finish Current
-      trans->status = I2CTransSuccess;
-
       if (ret == STMI2C_SubTra_Ready)
       {
         LED2_ON();
+        LED1_ON();
 	LED2_OFF();
+	LED1_OFF();
         // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
         regs->CR1 |= I2C_CR1_BIT_STOP;
 
@@ -677,9 +741,6 @@ static inline void i2c_error(struct i2c_periph *periph)
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
     periph->errors->arb_lost_cnt++;
     I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_ARLO);
-    //    I2C_AcknowledgeConfig(I2C2, DISABLE);
-    //    uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
-    //    I2C_GenerateSTOP(I2C2, ENABLE);
     err_nr = 3;
   }
   if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
@@ -705,7 +766,6 @@ static inline void i2c_error(struct i2c_periph *periph)
 
   LED_ERROR(err_nr);
 
-  i2c_event(periph);
   return;
   
 /*
@@ -769,7 +829,7 @@ void i2c1_ev_irq_handler(void) {
 }
 
 void i2c1_er_irq_handler(void) {
-  i2c_error(&i2c1);
+  i2c_event(&i2c1);
 }
 
 #endif /* USE_I2C1 */
@@ -845,7 +905,7 @@ void i2c2_ev_irq_handler(void) {
 }
 
 void i2c2_er_irq_handler(void) {
-  i2c_error(&i2c2);
+  i2c_event(&i2c2);
 }
 
 #endif /* USE_I2C2 */
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index ee8ce3db13..cbe8aef5da 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -49,11 +49,25 @@ static void hmc_send_config(uint8_t _init)
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   case 5:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.len_r = 2;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  case 6:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 2;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  case 7:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
     hmc5843.i2c_trans.type = I2CTransRx;
     hmc5843.i2c_trans.len_r = 2;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   default: 
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
     hmc5843.i2c_trans.type = I2CTransTxRx;
     hmc5843.i2c_trans.len_r = 2;
     hmc5843.i2c_trans.len_w = 1;
@@ -86,9 +100,9 @@ void hmc5843_idle_task(void)
   {
     LED_ON(5);
     LED_OFF(4);
-    if (hmc5843.initialized < 6)
+    if (hmc5843.initialized < 8)
     {
-       if (hmc5843.i2c_trans.status == I2CTransSuccess)
+       if ( (hmc5843.i2c_trans.status == I2CTransSuccess) || ( hmc5843.i2c_trans.slave_addr !=  HMC5843_ADDR))
        {
          hmc5843.initialized++;
        }
@@ -96,7 +110,7 @@ void hmc5843_idle_task(void)
      }
      else
      {
-
+       hmc5843.initialized = 0;
  	 
 /*
   // If transaction succeeded

From 8b7255e45a56034303adb19c2d6e92f07ce86bb5 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 21 Jul 2011 13:47:22 +0200
Subject: [PATCH 23/74] ReadMany

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 212 ++++++------------
 1 file changed, 63 insertions(+), 149 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 5f505830aa..4b98512c3c 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -58,8 +58,8 @@ static inline void LED_ERROR(uint8_t nr)
   LED2_OFF();
 }
 
-
-//////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
 
 #ifdef USE_I2C1
 static I2C_InitTypeDef  I2C1_InitStruct = {
@@ -83,21 +83,8 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 };
 #endif
 
-
-#ifdef USE_I2C2
-
-
-
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////
-
-// IDLE CHECK
-
-static bool_t PPRZ_I2C_IS_IDLE(struct i2c_periph* periph)
-{
-  return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
-}
 
 // (RE)START
 
@@ -346,77 +333,84 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
   return STMI2C_SubTra_Busy;
 }
 
-/*
-static inline enum STMI2CSubTransactionStatus stmi2c_handle_errors(I2C_TypeDef *regs, struct i2c_transaction *trans, struct i2c_periph *periph)
+static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
-  // Sensor did not Acknowlegde
-  if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_AF ))
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
-    // Disable Buffer interrupts
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    // Silent any BTF
-    regs->DR = 0x00; 
-
-    return STM_I2C_SubTra_Ready;
-
-    // Unless one is already scheduled: give a stop
-    if (periph->status != I2CStopRequested)
-    {
-      regs->CR1 |= I2C_CR1_BIT_STOP;
-    }
-    
-    // Followed by a fresh Start
-    regs->CR1 |= I2C_CR1_BIT_START;
+    // The first data byte will be acked in read many
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    // Clear the SB flag
+    regs->DR = trans->slave_addr | 0x01;
   }
-  else if (regs->SR1 & I2C_SR1_BITS_ERR)
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
-    // Disable Buffer interrupts
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Enable RXNE 
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    periph->idx_buf = 0;
 
-    // If we became slave (e.g. ARLO, etc ...)
-    if (! BIT_X_IS_SET_IN_REG( regs->SR2 , I2C_SR2_BIT_MSL ))
+    // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
+  {
+    // read everything until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
+    if (periph->idx_buf < (trans->len_r - 3))
     {
-      // Become master again
-      regs->CR1 |= I2C_CR1_BIT_START;
-      return;
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
     }
-
-    // If arbitration was lost (can only happen with many master / interference / SDA short)
-    // then STM I2C is back to slave mode. Issue a New Start right away as we are the only
-    // master to try again
-    if (BIT_X_IS_SET_IN_REG( regs->SR1 , I2C_SR1_BIT_ERR_ARLO ))
+    // this was the last byte
+    else if (periph->idx_buf == (trans->len_r - 1))
     {
-      regs->CR1 |= I2C_CR1_BIT_START;
-      return;
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+      // Last Value
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+
+      // We got all the results
+      trans->status = I2CTransSuccess;
+
+      return STMI2C_SubTra_Ready_StopRequested;
     }
-
-
-    // Shedule a Stop Condition, unless one was already sheduled: 
-    // then we would get a double stop and no interrupts
-    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_STOP ))
+    else
     {
+      // Stop listening to RXNE: ignore 1 until BTF is set and waits
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    }
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Now the shift register and data register contain data(n-2) and data(n-1)
+
+    // First we clear the ACK while the SCL is held low by BTF
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+
+    // Now the last byte is being clocked in MUST be set BEFORE reading the DR
+    // otherwise since there is new buffer space a new byte will be read
         LED2_ON();
 	LED2_OFF();
-      regs->CR1 |= I2C_CR1_BIT_STOP;
-    }
-    // Silent BTF Interrupts and shedule a byte of no bytes were left
-    regs->DR = 0x00;
+    regs->CR1 |= I2C_CR1_BIT_STOP;
 
-    // If not last: New Start
-    // If start already asked, then do nothing but wait for its interrupt
-    if (! BIT_X_IS_SET_IN_REG( regs->CR1 , I2C_CR1_BIT_START ))
-    {
-      regs->CR1 |= I2C_CR1_BIT_START;
-    }
 
-    // Reset WatchDog
-
-//    stage = 1 - stage;
-    return;
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+    
+    // The last byte will be received with RXNE
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
+
   return STMI2C_SubTra_Busy;
 }
-*/
+
 
 static inline void i2c_error(struct i2c_periph *periph);
 
@@ -665,63 +659,7 @@ static inline void i2c_event(struct i2c_periph *periph)
     }
   }
 
-
-
-
-
-
-
-
-
-
   return;
-
-/*
-
-  // Check to make sure that user space has an active transaction pending
-  if (periph->trans_extract_idx == periph->trans_insert_idx)
-  {
-    // no transaction?
-    periph->errors->unexpected_event_cnt++;
-    return;
-  }
-
-  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
-
-    // Periph was waiting for Start
-    if (periph->status == I2CStartRequested)
-    {
-      // Send Read Slave Address
-      if(trans->type == I2CTransRx)
-      {
-        I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
-      }
-      // Send Write Slave Address
-      else
-      {
-        I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
-      }
-      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
-    }
-    // Waiting for Restart: Always Rx
-    else if (periph->status == I2CRestartRequested)
-    {
-      I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
-      I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
-    }
-    // Problem: this problem need to be triggerd as if the 
-    // status was not OK then the buf size is also bad
-    else
-    {
-      PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
-    }
-    periph->status = I2CAddrWrSent;
-  }
-
-    // data is available every time RXNE is set. If BTF is set it means that 2 bytes are 
-    // ready to read (one in shift register) and I2C has stopped until the buffer can accept new data.
-
-*/
 }
 
 static inline void i2c_error(struct i2c_periph *periph)
@@ -768,26 +706,6 @@ static inline void i2c_error(struct i2c_periph *periph)
 
   return;
   
-/*
-
-  // Check to make sure that user space has an active transaction pending
-  if (periph->trans_extract_idx == periph->trans_insert_idx)
-  {
-    // no transaction?
-    periph->errors->unexpected_event_cnt++;
-    err_nr = 8;
-    return;
-  }
-
-  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
- //abort_and_reset(p);
-
-
-
-  PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransFailed);
-
-*/
-
 }
 
   
@@ -891,15 +809,11 @@ void i2c2_hw_init(void) {
 //  I2C_SoftwareResetCmd(I2C2, ENABLE);
 //  I2C_SoftwareResetCmd(I2C2, DISABLE);
 
-  // Reset and initialize I2C HW
   // enable error interrupts
   I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
 
-//  i2c_reset_init(&i2c2);
-
 }
 
-
 void i2c2_ev_irq_handler(void) {
   i2c_event(&i2c2);
 }
@@ -942,7 +856,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
 bool_t i2c_idle(struct i2c_periph* periph)
 {
-  return PPRZ_I2C_IS_IDLE(periph);
+  return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
   //return periph->status == I2CIdle;
 }
 

From b990e3b4602e31ede3572b5b95b70065bac06d84 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 17 Oct 2011 16:39:43 +0200
Subject: [PATCH 24/74] Reading the work done + commenting

---
 conf/airframes/CDW/debug_i2c.xml              |  2 +-
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 40 ++++++++++++++-----
 2 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
index 5d75d389b1..3e318c8ed5 100644
--- a/conf/airframes/CDW/debug_i2c.xml
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -236,7 +236,7 @@
     </subsystem>
     <!--subsystem name="ahrs"          type="cmpl"/-->
     <subsystem name="stabilization"   type="euler"/>
-    <subsystem name="ahrs"          type="ic"/>
+    <subsystem name="ahrs"          type="int_cmpl_quat"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 4b98512c3c..ae307b88e9 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -163,31 +163,36 @@ enum STMI2CSubTransactionStatus {
 
 static inline enum STMI2CSubTransactionStatus stmi2c_send1(I2C_TypeDef *regs, struct i2c_transaction *trans)
 {
+  // Read SR1 but wait reading SR2
   uint16_t SR1 = regs->SR1;
 
   // Start Condition Was Just Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
+    // No "buffer space available interrupt" as there is already space now: fill only 1 while space for 2 
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Send slave address and wait for address interrupt
     regs->DR = trans->slave_addr;
   }
-  // Address Was Sent
+  // Address Was Just Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // Now read SR2 to clear the ADDR
     volatile uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
 
-    // Send Bytes
+    // Send the Byte
     regs->DR = trans->buf[0];
 
     // BTF is set as soon as the shift register is empty. 
+    // BTF (ByteTransferFinished) means: I2C is halted: please urgently provide data
     // BTF is cleared A) when writing data to DR or B) when a start/stop condition OCCURRED (not was requested)
-    // Dummy Data to avoid BTF
+    // In the case of a RESTART: this fires a lot of unwanted interrupts:
+    // Force silent: Dummy Data to avoid BTF
     regs->DR = 0x00;
 
     if (trans->type == I2CTransTx)
     {
-      // We sent it all to the I2C ... might still be a chance that an error occurs
+      // We finished sending all to the I2C eninge ... ( might still be a chance that an error occurs )
       trans->status = I2CTransSuccess;
     }
 
@@ -204,7 +209,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs,
   // Start Condition Was Just Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
+    // Disable buffer interrupt
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Send Slave address and wait for ADDR interrupt
     regs->DR = trans->slave_addr;
   }
   // Address Was Sent
@@ -213,26 +220,30 @@ static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs,
     // Now read SR2 to clear the ADDR
     uint16_t SR2  __attribute__ ((unused)) = regs->SR2;
 
-    if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
+    // Maybe check we are transmitting (did not loose arbitration for instance)
+    // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
 
     // Send First 2 bytes
     regs->DR = trans->buf[0];
     regs->DR = trans->buf[1];
     periph->idx_buf = 2;
+    // Enable buffer-space available interrupt
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
+  // The buffer is not full anymore: (space for at least 1 and probably 1 is still transmitting)
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
   {
     // All bytes Sent?
     if ( periph->idx_buf >= trans->len_w)
     {
+      // Not interested anymore to know the buffer has space left
       regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
       // Also provide some dummy data in DR to silent the BTF interrupt
       regs->DR = 0x00;
 
       if (trans->type == I2CTransTx)
       {
-        // We sent it all to the I2C ... might still be a chance that an error occurs
+        // We finished sending all to the I2C eninge ... ( might still be a chance that an error occurs )
         trans->status = I2CTransSuccess;
       }
 
@@ -240,6 +251,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs,
     }
     else
     {
+      // Send the next byte
       regs->DR = trans->buf[periph->idx_buf];
       periph->idx_buf++;
     }
@@ -261,16 +273,18 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
-    // First Clear the ACK bit
+    // First Clear the ACK bit: after the next byte we do not want new bytes
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
+    // TODO: next to steps MUST be executed together to avoid missing the stop
+
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
     // Schedule a Stop
+    regs->CR1 |= I2C_CR1_BIT_STOP;
         LED2_ON();
 	LED2_OFF();
-    regs->CR1 |= I2C_CR1_BIT_STOP;
 
     // Enable the RXNE to get the result
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
@@ -280,7 +294,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     trans->buf[0] = regs->DR;
 
-    // We got all the results
+    // We got all the results (stop condition might still be in progress but this is the last interrupt)
     trans->status = I2CTransSuccess;
 
     return STMI2C_SubTra_Ready_StopRequested;
@@ -305,21 +319,25 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
+    // TODO: make absolutely sure this command is not delayed too much after the previous: 
+    //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
     // Disable the RXNE and wait for BTF
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
   }
+  // Receive buffer if full, master is halted: BTF
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
   {
     // Stop condition MUST be set BEFORE reading the DR
     // otherwise since there is new buffer space a new byte will be read
+    regs->CR1 |= I2C_CR1_BIT_STOP;
         LED2_ON();
 	LED2_OFF();
-    regs->CR1 |= I2C_CR1_BIT_STOP;
 
     trans->buf[0] = regs->DR;
     trans->buf[1] = regs->DR;
@@ -718,7 +736,7 @@ static inline void i2c_error(struct i2c_periph *periph)
   }
 */
 
-#endif /* USE_I2C2 */
+//#endif /* USE_I2C2 */
 
 
 

From 811b25e5d4a78185d62f9a7ebdfbd16d6626fe4e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 18 Oct 2011 16:58:57 +0200
Subject: [PATCH 25/74] Critical zones

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 71 +++++++++++++------
 1 file changed, 50 insertions(+), 21 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index ae307b88e9..7846a9fdc7 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -276,13 +276,16 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     // First Clear the ACK bit: after the next byte we do not want new bytes
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
-    // TODO: next to steps MUST be executed together to avoid missing the stop
+    // TODO: --- next to steps MUST be executed together to avoid missing the stop
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
     // Schedule a Stop
     regs->CR1 |= I2C_CR1_BIT_STOP;
+
+    // TODO --- end of critical zone -----------
+
         LED2_ON();
 	LED2_OFF();
 
@@ -322,13 +325,15 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
-    // TODO: make absolutely sure this command is not delayed too much after the previous: 
+    // TODO: --- make absolutely sure this command is not delayed too much after the previous: 
     //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
     // Disable the RXNE and wait for BTF
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    // TODO --- end of critical zone -----------
   }
   // Receive buffer if full, master is halted: BTF
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
@@ -359,7 +364,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    // The first data byte will be acked in read many
+    // The first data byte will be acked in read many so the slave knows it should send more
     regs->CR1 |= I2C_CR1_BIT_ACK;
     // Clear the SB flag
     regs->DR = trans->slave_addr | 0x01;
@@ -367,23 +372,28 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
-    // Enable RXNE 
+    // Enable RXNE: receive an interrupt any time a byte is available
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
     periph->idx_buf = 0;
 
-    // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    // ACK is still on to get more DATA
+    // Read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
   }
-  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
+  // one or more bytes are available AND we were interested in Buffer interrupts
+  // TODO: check if RXNE could be set when ITBUFEN is disabled
+  else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, CR2))
   {
-    // read everything until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
+    // read 1 byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
     if (periph->idx_buf < (trans->len_r - 3))
     {
       trans->buf[periph->idx_buf] = regs->DR;
       periph->idx_buf ++;
     }
-    // this was the last byte
-    else if (periph->idx_buf == (trans->len_r - 1))
+    // see else for intermetiate steps: 3bytes -> last byte
+    //
+    // finally: this was the last byte
+    else if (periph->idx_buf >= (trans->len_r - 1))
     {
       regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
 
@@ -396,35 +406,52 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
 
       return STMI2C_SubTra_Ready_StopRequested;
     }
-    else
+    // 3 bytes left to read
+    else // if (periph->idx_buf >= (trans->len_r - 3))
     {
-      // Stop listening to RXNE: ignore 1 until BTF is set and waits
+      // We want to halt I2C to have sufficient time to clear ACK, so:
+
+      // Do not read the received bytes just yet
+      // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer
+      // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted)
       regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     }
   }
+  // Buffer is full while this was not a RXNE interrupt
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
   {
     // Now the shift register and data register contain data(n-2) and data(n-1)
+    // And I2C is halted so we have time
 
     // First we clear the ACK while the SCL is held low by BTF
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
+    // TODO --- Make absolutely sure the next 2 I2C actions are performed with no delay
+
+    // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
 
-    // Now the last byte is being clocked in MUST be set BEFORE reading the DR
-    // otherwise since there is new buffer space a new byte will be read
+    // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
+    regs->CR1 |= I2C_CR1_BIT_STOP;
         LED2_ON();
 	LED2_OFF();
-    regs->CR1 |= I2C_CR1_BIT_STOP;
 
+    // TODO --- end of critical zone -----------
 
+    // read the second byte we had in the buffer (BTF means 2 bytes available)
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
     
+    // Ask for an interrupt to read the last byte (which is normally still busy now)
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
+  else
+  {
+    // TODO: catch
+  }
+
 
   return STMI2C_SubTra_Busy;
 }
@@ -450,14 +477,14 @@ static inline void i2c_event(struct i2c_periph *periph)
 	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
 
 	// Beware: using the buffered I2C has some interesting properties:
-	  -in receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
            in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
-	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost)
-	  -in transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
+	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
 	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
 	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
         
-        // Beware: besides buffering there is also event sheduling. You can send 2 bytes to the buffer, ask for a stop and 
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and 
            a new start in one go. 
 
           -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
@@ -470,7 +497,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 	If IT_EV_FEN AND IT_EV_BUF
 	--------------------------
 
-	Buffer event are not always wanted and are tipically switched on during longer data transfers. It highly depends on the data size.
+	Buffer event are not always wanted and are tipically switched on during longer data transfers. Make sure to turn off in time.
 
 	6) RxNE
 	7) TxE
@@ -483,6 +510,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
        // Status is re-used (abused) to remember the last COMMAND THAT WAS SENT to the STM I2C hardware.
 
+// TODO: check which are used
 	enum I2CStatus {
 	  I2CIdle,			// No more last command
 
@@ -512,8 +540,9 @@ static inline void i2c_event(struct i2c_periph *periph)
 	   not during data receiving when using buffered
 	   not after the last byte is received
 
-	The STM I2C stalls indefinately when a stop condition was attempted that
-	did not succeed. The BUSY flag remains on
+	-The STM I2C stalls indefinately when a stop condition was attempted that
+	did not succeed. The BUSY flag remains on.
+        -There is no STOP interrupt: use needs another way to finish.
 
    */
 

From 0e499f5da0fdfe4f27217372701b9bc9b9f29137 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 12:49:47 +0200
Subject: [PATCH 26/74] Powerfull new debugging tools

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 100 ++++++++++++++----
 sw/airborne/peripherals/hmc5843.c             |  17 +--
 2 files changed, 88 insertions(+), 29 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 7846a9fdc7..4566b5a4a5 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -20,7 +20,7 @@ static inline void LED1_OFF(void)
   GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
 }
 
-static inline void LED2_ON()
+static inline void LED2_ON(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
 }
@@ -47,17 +47,15 @@ static inline void LED_INIT(void)
 static inline void LED_ERROR(uint8_t nr)
 {
   LED2_ON();
-  for (int i=0;i<20;i++)
+  for (int i=0;i<(20+nr);i++)
   {
-    if (nr == i)
-      LED1_OFF();
-    else
-      LED1_ON();
+    LED1_ON();
     LED1_OFF();    
   }
   LED2_OFF();
 }
 
+
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
@@ -90,10 +88,19 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 
 static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+
   periph->idx_buf = 0;
   periph->status = I2CStartRequested;
   // Clear any pending stop
-  I2C_GenerateSTOP(periph->reg_addr, DISABLE);
+  // I2C_GenerateSTOP(periph->reg_addr, DISABLE);
   // Issue a new start
   I2C_GenerateSTART(periph->reg_addr, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
@@ -150,6 +157,44 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 
 #define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
 
+// TODO: remove debug
+
+static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
+{
+  LED1_ON();
+
+  // Start
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // Addr
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // BTF
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // ERROR
+  if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+
+  LED1_OFF();
+}
+
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
 //
@@ -238,14 +283,17 @@ static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs,
     {
       // Not interested anymore to know the buffer has space left
       regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-      // Also provide some dummy data in DR to silent the BTF interrupt
-      regs->DR = 0x00;
 
       if (trans->type == I2CTransTx)
       {
         // We finished sending all to the I2C eninge ... ( might still be a chance that an error occurs )
         trans->status = I2CTransSuccess;
       }
+      //else
+      {
+        // Also provide some dummy data in DR to silent the BTF interrupt
+        regs->DR = 0x00;
+      }
 
       return STMI2C_SubTra_Ready;
     }
@@ -382,7 +430,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   }
   // one or more bytes are available AND we were interested in Buffer interrupts
   // TODO: check if RXNE could be set when ITBUFEN is disabled
-  else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, CR2))
+  else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
   {
     // read 1 byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
     if (periph->idx_buf < (trans->len_r - 3))
@@ -584,14 +632,17 @@ static inline void i2c_event(struct i2c_periph *periph)
     periph->status = I2CIdle;
     periph->errors->unexpected_event_cnt++;
 
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
         LED2_ON();
         LED1_ON();
-	LED1_OFF();
 	LED2_OFF();
-    regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
-    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
-    regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
+	LED1_OFF();
+    //regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
+    //uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
+    //regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
+
+    // no transaction and also an error?
+    LED_SHOW_ACTIVE_BITS(regs->SR1);
 
     return;
   }
@@ -605,14 +656,14 @@ static inline void i2c_event(struct i2c_periph *periph)
 
     // Close the Bus
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
-    regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
+    //regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
         LED1_ON();
         LED2_ON();
-	LED2_OFF();
 	LED1_OFF();
-    periph->status = I2CStopRequested;
-    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
-    regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
+	LED2_OFF();
+    //periph->status = I2CStopRequested;
+    //uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
+    //regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
 
     // Prepare for next
     ret = STMI2C_SubTra_Ready_StopRequested;
@@ -668,10 +719,11 @@ static inline void i2c_event(struct i2c_periph *periph)
     {
       if (ret == STMI2C_SubTra_Ready)
       {
+        // Program a stop
         LED2_ON();
         LED1_ON();
-	LED2_OFF();
 	LED1_OFF();
+	LED2_OFF();
         // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
         regs->CR1 |= I2C_CR1_BIT_STOP;
 
@@ -687,6 +739,12 @@ static inline void i2c_event(struct i2c_periph *periph)
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
         periph->status = I2CIdle;
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
       }
       // if not, start next transaction
       else
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index cbe8aef5da..9589cbed26 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -19,6 +19,9 @@ void hmc5843_init(void)
 
 static void hmc_send_config(uint8_t _init)
 {
+  hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+  hmc5843.i2c_trans.len_w = 0;
+  hmc5843.i2c_trans.len_r = 0;
   switch (_init)
   {
   case 1:
@@ -31,7 +34,7 @@ static void hmc_send_config(uint8_t _init)
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
     break;
   case 2:
-    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
     hmc5843.i2c_trans.buf[1] = 0x01<<5;
     hmc5843.i2c_trans.len_w = 2;
@@ -49,21 +52,19 @@ static void hmc_send_config(uint8_t _init)
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   case 5:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
-    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.type = I2CTransRx;
     hmc5843.i2c_trans.len_r = 2;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   case 6:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
     hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 2;
+    hmc5843.i2c_trans.len_r = 3;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   case 7:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 2;
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   default: 

From 4e7ef19dbd1002b20d59668604ad2480042002b9 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 13:06:05 +0200
Subject: [PATCH 27/74] Loose a lot of efficiency: delay the last
 master_transmit interrupt to BTF instead of TXE: then we know the
 transmission was succesfull or not.

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 49 +++++++++++--------
 1 file changed, 28 insertions(+), 21 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 4566b5a4a5..25774e40c6 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -268,41 +268,48 @@ static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs,
     // Maybe check we are transmitting (did not loose arbitration for instance)
     // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
 
-    // Send First 2 bytes
+    // Send First max 2 bytes
     regs->DR = trans->buf[0];
-    regs->DR = trans->buf[1];
-    periph->idx_buf = 2;
+    if (trans->len_w > 1)
+    {
+      regs->DR = trans->buf[1];
+      periph->idx_buf = 2;
+    }
+    else
+    {
+      periph->idx_buf = 1;
+    }
+
     // Enable buffer-space available interrupt
-    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    // only if there is more to send: wait for TXE, no more to send: wait for BTF
+    if ( periph->idx_buf < trans->len_w)
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
-  // The buffer is not full anymore: (space for at least 1 and probably 1 is still transmitting)
-  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) )
+  // The buffer is not full anymore AND we were not waiting for BTF
+  else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
   {
-    // All bytes Sent?
+    // Send the next byte
+    regs->DR = trans->buf[periph->idx_buf];
+    periph->idx_buf++;
+
+    // All bytes Sent? Then wait for BTF instead
     if ( periph->idx_buf >= trans->len_w)
     {
       // Not interested anymore to know the buffer has space left
       regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-
+      // Next interrupt will be BTF (or error)
+    }
+  }
+  // BTF: means last byte was sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
       if (trans->type == I2CTransTx)
       {
-        // We finished sending all to the I2C eninge ... ( might still be a chance that an error occurs )
+        // Tell the driver we are ready
         trans->status = I2CTransSuccess;
       }
-      //else
-      {
-        // Also provide some dummy data in DR to silent the BTF interrupt
-        regs->DR = 0x00;
-      }
 
       return STMI2C_SubTra_Ready;
-    }
-    else
-    {
-      // Send the next byte
-      regs->DR = trans->buf[periph->idx_buf];
-      periph->idx_buf++;
-    }
   }
 
   return STMI2C_SubTra_Busy;

From 7a946dfbbd80af0bf190ba3a28e79f26872d0759 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 13:16:05 +0200
Subject: [PATCH 28/74] Send1 Removed. Still sadd about the big loss off
 efficiency that BTF is causing: on MKK motor controller lines you loose 10%
 :-(

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 52 +------------------
 1 file changed, 2 insertions(+), 50 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 25774e40c6..e5403b0a60 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -206,48 +206,7 @@ enum STMI2CSubTransactionStatus {
   STMI2C_SubTra_Ready
 };
 
-static inline enum STMI2CSubTransactionStatus stmi2c_send1(I2C_TypeDef *regs, struct i2c_transaction *trans)
-{
-  // Read SR1 but wait reading SR2
-  uint16_t SR1 = regs->SR1;
-
-  // Start Condition Was Just Generated
-  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
-  {
-    // No "buffer space available interrupt" as there is already space now: fill only 1 while space for 2 
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
-    // Send slave address and wait for address interrupt
-    regs->DR = trans->slave_addr;
-  }
-  // Address Was Just Sent
-  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
-  {
-    // Now read SR2 to clear the ADDR
-    volatile uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-
-    // Send the Byte
-    regs->DR = trans->buf[0];
-
-    // BTF is set as soon as the shift register is empty. 
-    // BTF (ByteTransferFinished) means: I2C is halted: please urgently provide data
-    // BTF is cleared A) when writing data to DR or B) when a start/stop condition OCCURRED (not was requested)
-    // In the case of a RESTART: this fires a lot of unwanted interrupts:
-    // Force silent: Dummy Data to avoid BTF
-    regs->DR = 0x00;
-
-    if (trans->type == I2CTransTx)
-    {
-      // We finished sending all to the I2C eninge ... ( might still be a chance that an error occurs )
-      trans->status = I2CTransSuccess;
-    }
-
-    return STMI2C_SubTra_Ready;
-  }
-
-  return STMI2C_SubTra_Busy;
-}
-
-static inline enum STMI2CSubTransactionStatus stmi2c_sendmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
   uint16_t SR1 = regs->SR1;
 
@@ -699,14 +658,7 @@ static inline void i2c_event(struct i2c_periph *periph)
     }
     else // TxRx or Tx
     {
-      if (trans->len_w > 1)
-      {
-        ret = stmi2c_sendmany(regs,periph,trans);
-      }    
-      else
-      {
-        ret = stmi2c_send1(regs,trans);
-      }
+      ret = stmi2c_send(regs,periph,trans);
       if (trans->type == I2CTransTxRx)
       {
         restart = 1;

From 817407cc5bacd7ebc9a59520586f300b29d2c42e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 15:28:23 +0200
Subject: [PATCH 29/74] start with 1 driver at a time

---
 sw/airborne/boards/lisa_l/baro_board.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c
index 11829b5acf..807217e5e2 100644
--- a/sw/airborne/boards/lisa_l/baro_board.c
+++ b/sw/airborne/boards/lisa_l/baro_board.c
@@ -26,6 +26,8 @@ void baro_init(void) {
 
 void baro_periodic(void) {
   // check i2c_done
+  return; 
+
   if (!i2c_idle(&i2c2)) return;
   switch (baro_board.status) {
   case LBS_UNINITIALIZED:

From 9f7dc65123e27f3307710b54af9c73ba39fe2afc Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 15:29:08 +0200
Subject: [PATCH 30/74] Significantly improved error handling ...

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 83 +++++++++++++++++--
 sw/airborne/peripherals/hmc5843.c             | 54 ++++--------
 2 files changed, 89 insertions(+), 48 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index e5403b0a60..3882e7f8f0 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -77,7 +77,8 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 400000
+      .I2C_ClockSpeed = 37000
+//      .I2C_ClockSpeed = 400000
 };
 #endif
 
@@ -300,7 +301,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
 
     // TODO --- end of critical zone -----------
 
+        // Program a stop
         LED2_ON();
+        LED1_ON();
+	LED1_OFF();
 	LED2_OFF();
 
     // Enable the RXNE to get the result
@@ -355,7 +359,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // Stop condition MUST be set BEFORE reading the DR
     // otherwise since there is new buffer space a new byte will be read
     regs->CR1 |= I2C_CR1_BIT_STOP;
+        // Program a stop
         LED2_ON();
+        LED1_ON();
+	LED1_OFF();
 	LED2_OFF();
 
     trans->buf[0] = regs->DR;
@@ -448,7 +455,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
 
     // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
     regs->CR1 |= I2C_CR1_BIT_STOP;
+        // Program a stop
         LED2_ON();
+        LED1_ON();
+	LED1_OFF();
 	LED2_OFF();
 
     // TODO --- end of critical zone -----------
@@ -471,6 +481,34 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
 }
 
 
+static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
+{
+  uint16_t SR1 = regs->SR1;
+
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+  // Start Condition Was Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // SB: cleared by software when reading SR1 and writing to DR
+    regs->DR = 0x00;
+  }
+  // Address Was Sent
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // ADDR: Cleared by software when reading SR1 and then SR2
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // Byte Transfer Finished
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // SB: cleared by software when reading SR1 and reading/writing to DR
+    uint8_t dummy __attribute__ ((unused)) = regs->DR;
+    regs->DR = 0x00;
+  }
+}
+
+
 static inline void i2c_error(struct i2c_periph *periph);
 
 static inline void i2c_event(struct i2c_periph *periph)
@@ -603,13 +641,13 @@ static inline void i2c_event(struct i2c_periph *periph)
         LED1_ON();
 	LED2_OFF();
 	LED1_OFF();
-    //regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
-    //uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
-    //regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
 
     // no transaction and also an error?
     LED_SHOW_ACTIVE_BITS(regs->SR1);
 
+    stmi2c_clear_pending_interrupts(regs);
+    i2c_error(periph);
+
     return;
   }
 
@@ -622,19 +660,22 @@ static inline void i2c_event(struct i2c_periph *periph)
 
     // Close the Bus
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
-    //regs->CR1 |= I2C_CR1_BIT_STOP;			// Issue a stop
+
+
         LED1_ON();
         LED2_ON();
 	LED1_OFF();
 	LED2_OFF();
-    //periph->status = I2CStopRequested;
+
     //uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
     //regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
 
     // Prepare for next
-    ret = STMI2C_SubTra_Ready_StopRequested;
+    ret = STMI2C_SubTra_Ready;
     restart = 0;
 
+    stmi2c_clear_pending_interrupts(regs);
+
     // Count it
     i2c_error(periph);
   }
@@ -686,6 +727,9 @@ static inline void i2c_event(struct i2c_periph *periph)
         // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
         regs->CR1 |= I2C_CR1_BIT_STOP;
 
+        // Silent any BTF that would occur before STOP is executed
+        regs->DR = 0x00;
+
         periph->status = I2CStopRequested;
       }
 
@@ -720,6 +764,9 @@ static inline void i2c_event(struct i2c_periph *periph)
       trans->type = I2CTransRx;
       periph->status = I2CStartRequested;
       regs->CR1 |= I2C_CR1_BIT_START;
+
+      // Silent any BTF that would occur before SB
+      regs->DR = 0x00;
     }
   }
 
@@ -911,7 +958,27 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
   /* if peripheral is idle, start the transaction */
   // if (PPRZ_I2C_IS_IDLE(p))
   if (periph->status == I2CIdle)
-    PPRZ_I2C_SEND_START(periph);
+  {
+	if (periph == &i2c1)
+	{
+/*
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+*/
+        }
+        else
+        {
+          PPRZ_I2C_SEND_START(periph);
+        }
+  }
   /* else it will be started by the interrupt handler when the previous transactions completes */
   __enable_irq();
 
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 9589cbed26..6fb5463f43 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -2,7 +2,7 @@
 
 #include "led.h"
 
-#define HMC5843_TIMEOUT 10
+#define HMC5843_TIMEOUT 100
 
 #define bswap_16(x)   ((((x) & 0xFF00) >> 8) | (((x) & 0x00FF) << 8))
 
@@ -15,6 +15,7 @@ void hmc5843_init(void)
   hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
 
   hmc5843_arch_init();
+  hmc5843.initialized = 0;
 }
 
 static void hmc_send_config(uint8_t _init)
@@ -79,12 +80,6 @@ static void hmc_send_config(uint8_t _init)
 
 void hmc5843_idle_task(void)
 {
-  if (hmc5843.timeout > HMC5843_TIMEOUT)
-  {
-//    hmc5843.timeout = 0;
-//    LED_TOGGLE(4);
-  }
-
   if (i2c_idle(&i2c2))
   {
     LED_ON(7);	// green = idle
@@ -101,41 +96,20 @@ void hmc5843_idle_task(void)
   {
     LED_ON(5);
     LED_OFF(4);
-    if (hmc5843.initialized < 8)
+    if (hmc5843.initialized < 9)
     {
-       if ( (hmc5843.i2c_trans.status == I2CTransSuccess) || ( hmc5843.i2c_trans.slave_addr !=  HMC5843_ADDR))
-       {
-         hmc5843.initialized++;
-       }
-       hmc_send_config( hmc5843.initialized );
-     }
-     else
-     {
-       hmc5843.initialized = 0;
- 	 
-/*
-  // If transaction succeeded
-  if (hmc5843.i2c_trans.status == I2CTransSuccess) 
-  {
-    memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
-    for (int i = 0; i < 3; i++) {
-      hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
+       hmc5843.initialized++;
     }
-    hmc5843.data_available = TRUE;
-  }
-
-  // Start a new one
-  if (hmc5843.timeout > HMC5843_TIMEOUT)
-  {
-
-    hmc5843.timeout = 0;
-*/
-      hmc5843.i2c_trans.type = I2CTransTxRx;
-      hmc5843.i2c_trans.len_r = 6;
-      hmc5843.i2c_trans.len_w = 1;
-      hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-      i2c_submit(&i2c2, &hmc5843.i2c_trans);
+    else
+    {
+      hmc5843.initialized = 1;
     }
+
+    if (hmc5843.initialized == 1)
+    {
+      hmc_send_config( hmc5843.initialized ); 	 
+    }
+
   }
   else
   {
@@ -147,5 +121,5 @@ void hmc5843_idle_task(void)
 
 void hmc5843_periodic(void)
 {
-  hmc5843.timeout++;
+  //hmc5843.timeout++;
 }

From 9081b739d6aef931bc7b438db55a1dcccd653c41 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 17:01:36 +0200
Subject: [PATCH 31/74] NACK not cleared in ReadMany. Double Stop condition
 after error while bit only set once...

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 46 ++++++++++++-------
 sw/airborne/peripherals/hmc5843.c             |  9 +++-
 2 files changed, 36 insertions(+), 19 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 3882e7f8f0..aba5604fe3 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -100,8 +100,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 
   periph->idx_buf = 0;
   periph->status = I2CStartRequested;
-  // Clear any pending stop
-  // I2C_GenerateSTOP(periph->reg_addr, DISABLE);
+
   // Issue a new start
   I2C_GenerateSTART(periph->reg_addr, ENABLE);
   I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
@@ -291,7 +290,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     // First Clear the ACK bit: after the next byte we do not want new bytes
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
-    // TODO: --- next to steps MUST be executed together to avoid missing the stop
+    // --- next to steps MUST be executed together to avoid missing the stop
+    __disable_irq();
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
@@ -299,7 +299,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     // Schedule a Stop
     regs->CR1 |= I2C_CR1_BIT_STOP;
 
-    // TODO --- end of critical zone -----------
+    __enable_irq();
+    // --- end of critical zone -----------
 
         // Program a stop
         LED2_ON();
@@ -343,7 +344,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
-    // TODO: --- make absolutely sure this command is not delayed too much after the previous: 
+    // --- make absolutely sure this command is not delayed too much after the previous: 
+    __disable_irq();
     //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
@@ -351,7 +353,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // Disable the RXNE and wait for BTF
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
 
-    // TODO --- end of critical zone -----------
+    __enable_irq();
+    // --- end of critical zone -----------
   }
   // Receive buffer if full, master is halted: BTF
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
@@ -394,7 +397,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // Enable RXNE: receive an interrupt any time a byte is available
-    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    // only enable if MORE than 3 bytes need to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+
     periph->idx_buf = 0;
 
     // ACK is still on to get more DATA
@@ -405,13 +411,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   // TODO: check if RXNE could be set when ITBUFEN is disabled
   else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
   {
-    // read 1 byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
+    // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
     if (periph->idx_buf < (trans->len_r - 3))
     {
       trans->buf[periph->idx_buf] = regs->DR;
       periph->idx_buf ++;
     }
-    // see else for intermetiate steps: 3bytes -> last byte
+    // from : 3bytes -> last byte: do nothing
     //
     // finally: this was the last byte
     else if (periph->idx_buf >= (trans->len_r - 1))
@@ -427,12 +433,15 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
 
       return STMI2C_SubTra_Ready_StopRequested;
     }
-    // 3 bytes left to read
-    else // if (periph->idx_buf >= (trans->len_r - 3))
+
+    // Check for end of transaction: start waiting for BTF instead of RXNE
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+    else // idx >= len-3: there are 3 bytes to be read
     {
       // We want to halt I2C to have sufficient time to clear ACK, so:
-
-      // Do not read the received bytes just yet
       // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer
       // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted)
       regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
@@ -447,7 +456,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // First we clear the ACK while the SCL is held low by BTF
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
-    // TODO --- Make absolutely sure the next 2 I2C actions are performed with no delay
+    // --- Make absolutely sure the next 2 I2C actions are performed with no delay
+    __disable_irq();
 
     // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
     trans->buf[periph->idx_buf] = regs->DR;
@@ -461,9 +471,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
 	LED1_OFF();
 	LED2_OFF();
 
-    // TODO --- end of critical zone -----------
+    __enable_irq();
+    // --- end of critical zone -----------
 
-    // read the second byte we had in the buffer (BTF means 2 bytes available)
+    // read the byte2 we had in the buffer (BTF means 2 bytes available)
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
     
@@ -648,6 +659,7 @@ static inline void i2c_event(struct i2c_periph *periph)
     stmi2c_clear_pending_interrupts(regs);
     i2c_error(periph);
 
+    // There are no transactions anymore: so we are not allowed to continue
     return;
   }
 
@@ -693,7 +705,7 @@ static inline void i2c_event(struct i2c_periph *periph)
           ret = stmi2c_read2(regs,trans);
           break;
         default:
-          ret = stmi2c_read2(regs,trans);
+          ret = stmi2c_readmany(regs,periph, trans);
           break;    
       }
     }
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 6fb5463f43..8f62bc750f 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -68,10 +68,15 @@ static void hmc_send_config(uint8_t _init)
     hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
+  case 8:
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 4;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
   default: 
     hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
     hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 2;
+    hmc5843.i2c_trans.len_r = 4;
     hmc5843.i2c_trans.len_w = 1;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
     i2c_submit(&i2c2, &hmc5843.i2c_trans);
@@ -105,7 +110,7 @@ void hmc5843_idle_task(void)
       hmc5843.initialized = 1;
     }
 
-    if (hmc5843.initialized == 1)
+//    if (hmc5843.initialized == 1)
     {
       hmc_send_config( hmc5843.initialized ); 	 
     }

From 04a7f31f86126b230ba80812fd462eacc523e581 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 17:31:51 +0200
Subject: [PATCH 32/74] Haaaa! I had forgotten to clear the POS bit in readmany
 that was sometimes still set from read2. ReadMany now NACKs just as expected.

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index aba5604fe3..ba91e7951a 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -288,6 +288,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // First Clear the ACK bit: after the next byte we do not want new bytes
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
     // --- next to steps MUST be executed together to avoid missing the stop
@@ -389,6 +390,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     // The first data byte will be acked in read many so the slave knows it should send more
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
     regs->CR1 |= I2C_CR1_BIT_ACK;
     // Clear the SB flag
     regs->DR = trans->slave_addr | 0x01;
@@ -453,11 +455,18 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // Now the shift register and data register contain data(n-2) and data(n-1)
     // And I2C is halted so we have time
 
+    // --- Make absolutely sure the next 2 I2C actions are performed with no delay
+    __disable_irq();
+
     // First we clear the ACK while the SCL is held low by BTF
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
-    // --- Make absolutely sure the next 2 I2C actions are performed with no delay
-    __disable_irq();
+        LED2_ON();
+        LED1_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED1_OFF();
+	LED2_OFF();
 
     // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
     trans->buf[periph->idx_buf] = regs->DR;
@@ -482,11 +491,6 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
-  else
-  {
-    // TODO: catch
-  }
-
 
   return STMI2C_SubTra_Busy;
 }

From 9f81ca78f0e28be7cbff050cfaae5061c59eef32 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 18:20:29 +0200
Subject: [PATCH 33/74] More Test Programs -> Removed bug in Read-Many when
 used in TxRx

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 27 ++++---
 sw/airborne/peripherals/hmc5843.c             | 75 +++++++++++++++++--
 2 files changed, 84 insertions(+), 18 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index ba91e7951a..ee376b3720 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -44,10 +44,10 @@ static inline void LED_INIT(void)
 }
 
 
-static inline void LED_ERROR(uint8_t nr)
+static inline void LED_ERROR(uint8_t base, uint8_t nr)
 {
   LED2_ON();
-  for (int i=0;i<(20+nr);i++)
+  for (int i=0;i<(base+nr);i++)
   {
     LED1_ON();
     LED1_OFF();    
@@ -398,12 +398,14 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
+    periph->idx_buf = 0;
+
     // Enable RXNE: receive an interrupt any time a byte is available
     // only enable if MORE than 3 bytes need to be read
     if (periph->idx_buf < (trans->len_r - 3))
+    {
       regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
-
-    periph->idx_buf = 0;
+    }
 
     // ACK is still on to get more DATA
     // Read SR2 to clear the ADDR (next byte will start arriving)
@@ -461,13 +463,6 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // First we clear the ACK while the SCL is held low by BTF
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
-        LED2_ON();
-        LED1_ON();
-        LED1_ON();
-	LED1_OFF();
-	LED1_OFF();
-	LED2_OFF();
-
     // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
@@ -491,6 +486,14 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
+  else
+  {
+    // Error
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+  }
 
   return STMI2C_SubTra_Busy;
 }
@@ -829,7 +832,7 @@ static inline void i2c_error(struct i2c_periph *periph)
     err_nr = 7;
   }
 
-  LED_ERROR(err_nr);
+  LED_ERROR(20, err_nr);
 
   return;
   
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 8f62bc750f..3888949c1d 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -9,6 +9,9 @@
 struct Hmc5843 hmc5843;
 void exti9_5_irq_handler(void);
 
+struct i2c_transaction i2c_test2;
+
+
 void hmc5843_init(void)
 {
   hmc5843.i2c_trans.status = I2CTransSuccess;
@@ -20,6 +23,12 @@ void hmc5843_init(void)
 
 static void hmc_send_config(uint8_t _init)
 {
+  //i2c_test2.slave_addr = 0x90;
+  i2c_test2.type = I2CTransTx;
+  i2c_test2.slave_addr = 0x00;
+  i2c_test2.len_w = 1;
+  i2c_test2.buf[0] = 0x06;
+
   hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
   hmc5843.i2c_trans.len_w = 0;
   hmc5843.i2c_trans.len_r = 0;
@@ -63,20 +72,74 @@ static void hmc_send_config(uint8_t _init)
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
   case 7:
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 4;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  case 8:
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 5;
+    i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  break;
+  case 9:
+    // bad addr
     hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
     hmc5843.i2c_trans.type = I2CTransTx;
     hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
   break;
-  case 8:
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 4;
+  case 10:
+    // 2 consecutive
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to continuous mode
+    hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
+    i2c_submit(&i2c2,&i2c_test2);
+  break;
+  case 11:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 1;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+  break;
+  case 12:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 2;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+  break;
+  case 13:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 3;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+  break;
+  case 14:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 4;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+  break;
+  case 15:
+    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+    hmc5843.i2c_trans.type = I2CTransTxRx;
+    hmc5843.i2c_trans.len_r = 4;
+    hmc5843.i2c_trans.len_w = 2;
+    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
   break;
   default: 
     hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
     hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 4;
+    hmc5843.i2c_trans.len_r = 5;
     hmc5843.i2c_trans.len_w = 1;
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
     i2c_submit(&i2c2, &hmc5843.i2c_trans);
@@ -101,7 +164,7 @@ void hmc5843_idle_task(void)
   {
     LED_ON(5);
     LED_OFF(4);
-    if (hmc5843.initialized < 9)
+    if (hmc5843.initialized < 16)
     {
        hmc5843.initialized++;
     }
@@ -110,7 +173,7 @@ void hmc5843_idle_task(void)
       hmc5843.initialized = 1;
     }
 
-//    if (hmc5843.initialized == 1)
+    //if (hmc5843.initialized < 4)
     {
       hmc_send_config( hmc5843.initialized ); 	 
     }

From 02021e7270690c8b36aa6901e66fd8ee5edb9ca2 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 21:21:16 +0200
Subject: [PATCH 34/74] Automatically Check All transation types T1 T2 T3 T4 R1
 R2 R3 R4 R5 T1R1 T1R2 T1R3 T1R4 T2R1 T2R2 Twrong_addr Rwrong_addr, and all
 automatically check all bitrates from 10k to 800kbps

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 85 +++++++++++++------
 sw/airborne/mcu_periph/i2c.h                  |  1 +
 sw/airborne/peripherals/hmc5843.c             | 55 +++++++-----
 3 files changed, 92 insertions(+), 49 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index ee376b3720..ec9f211226 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -77,39 +77,14 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 37000
-//      .I2C_ClockSpeed = 400000
+//      .I2C_ClockSpeed = 37000
+      .I2C_ClockSpeed = 400000
 };
 #endif
 
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
-// (RE)START
-
-static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
-{
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-
-  periph->idx_buf = 0;
-  periph->status = I2CStartRequested;
-
-  // Issue a new start
-  I2C_GenerateSTART(periph->reg_addr, ENABLE);
-  I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
-  I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////
-
 // Bypassing the libSTM I2C functions to have more control over the reading of registers
 // e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
 
@@ -157,6 +132,54 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 
 #define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
 
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// (RE)START
+
+static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  periph->idx_buf = 0;
+  periph->status = I2CStartRequested;
+
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
+  {
+    regs->CR1 &= ~ I2C_CR1_BIT_STOP;
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+  }
+  else
+  {
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+  }
+
+  // Issue a new start
+  regs->CR1 |=  I2C_CR1_BIT_START;
+  
+  // Enable Error IRQ, Event IRQ but disable Buffer IRQ
+  regs->CR2 |= I2C_CR2_BIT_ITERREN;
+  regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+}
+
 // TODO: remove debug
 
 static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
@@ -944,6 +967,14 @@ void i2c2_hw_init(void) {
 
 }
 
+void i2c2_setbitrate(int bitrate)
+{
+  I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+  I2C_Init(I2C2, i2c2.init_struct);
+}
+
+
+
 void i2c2_ev_irq_handler(void) {
   i2c_event(&i2c2);
 }
diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h
index 93bd0f77ba..d5e2595563 100644
--- a/sw/airborne/mcu_periph/i2c.h
+++ b/sw/airborne/mcu_periph/i2c.h
@@ -127,6 +127,7 @@ extern void i2c1_init(void);
 
 extern struct i2c_periph i2c2;
 extern void i2c2_init(void);
+extern void i2c2_setbitrate(int bitrate);
 
 #endif /* USE_I2C2 */
 
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 3888949c1d..ab4e105ae6 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -19,6 +19,7 @@ void hmc5843_init(void)
 
   hmc5843_arch_init();
   hmc5843.initialized = 0;
+  hmc5843.timeout = 10000;
 }
 
 static void hmc_send_config(uint8_t _init)
@@ -94,7 +95,7 @@ static void hmc_send_config(uint8_t _init)
     hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to continuous mode
     hmc5843.i2c_trans.len_w = 1;
     i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    i2c_submit(&i2c2,&i2c_test2);
+    //i2c_submit(&i2c2,&i2c_test2);
   break;
   case 11:
     hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
@@ -160,31 +161,41 @@ void hmc5843_idle_task(void)
   }
 
   // Wait for I2C transaction object to be released by the I2C driver before changing anything
-  if ((hmc5843.i2c_trans.status == I2CTransFailed) || (hmc5843.i2c_trans.status == I2CTransSuccess))
+  if (i2c_idle(&i2c2))
   {
-    LED_ON(5);
-    LED_OFF(4);
-    if (hmc5843.initialized < 16)
-    {
-       hmc5843.initialized++;
-    }
-    else
-    {
-      hmc5843.initialized = 1;
-    }
+	  if ((hmc5843.i2c_trans.status == I2CTransFailed) || (hmc5843.i2c_trans.status == I2CTransSuccess))
+	  {
+	    LED_ON(5);
+	    LED_OFF(4);
+	    if (hmc5843.initialized < 16)
+	    {
+	       hmc5843.initialized++;
+	    }
+	    else
+	    {
+	      hmc5843.initialized = 1;
 
-    //if (hmc5843.initialized < 4)
-    {
-      hmc_send_config( hmc5843.initialized ); 	 
-    }
+	      i2c2_setbitrate(hmc5843.timeout);
 
+	      hmc5843.timeout += 10000;
+              if (hmc5843.timeout > 800000)
+              {
+                hmc5843.timeout = 10000;
+              }
+	    }
+
+	    //if (hmc5843.initialized < 4)
+	    {
+	      hmc_send_config( hmc5843.initialized ); 	 
+	    }
+
+	  }
+	  else
+	  {
+	    LED_ON(4);
+	    LED_OFF(5);
+	  }
   }
-  else
-  {
-    LED_ON(4);
-    LED_OFF(5);
-  }
-
 }
 
 void hmc5843_periodic(void)

From 5cea02d537092ca9738ef610b2f116aad0eabec5 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 19 Oct 2011 21:48:10 +0200
Subject: [PATCH 35/74] Option to remove all DEBUG_LEDs and see if that still
 works: it seems so...

---
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 52 +++++++++++++------
 1 file changed, 36 insertions(+), 16 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index ec9f211226..25337d747b 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -7,9 +7,13 @@
 
 //#include "led.h" 
 
+//#define I2C_DEBUG_LED
+
 /////////// DEBUGGING //////////////
 // TODO: remove this 
 
+
+#ifdef I2C_DEBUG_LED
 static inline void LED1_ON(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
@@ -54,7 +58,7 @@ static inline void LED_ERROR(uint8_t base, uint8_t nr)
   }
   LED2_OFF();
 }
-
+#endif
 
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
@@ -148,6 +152,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   {
     regs->CR1 &= ~ I2C_CR1_BIT_STOP;
 
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
@@ -158,9 +163,11 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
   }
   else
   {
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
@@ -169,6 +176,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
   }
 
   // Issue a new start
@@ -181,6 +189,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 }
 
 // TODO: remove debug
+#ifdef I2C_DEBUG_LED
 
 static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
 {
@@ -217,6 +226,7 @@ static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
 
   LED1_OFF();
 }
+#endif
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -326,11 +336,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     __enable_irq();
     // --- end of critical zone -----------
 
+#ifdef I2C_DEBUG_LED
         // Program a stop
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
 
     // Enable the RXNE to get the result
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
@@ -387,10 +399,12 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // otherwise since there is new buffer space a new byte will be read
     regs->CR1 |= I2C_CR1_BIT_STOP;
         // Program a stop
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
 
     trans->buf[0] = regs->DR;
     trans->buf[1] = regs->DR;
@@ -435,7 +449,6 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
   }
   // one or more bytes are available AND we were interested in Buffer interrupts
-  // TODO: check if RXNE could be set when ITBUFEN is disabled
   else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
   {
     // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
@@ -493,10 +506,12 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
     regs->CR1 |= I2C_CR1_BIT_STOP;
         // Program a stop
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
 
     __enable_irq();
     // --- end of critical zone -----------
@@ -512,10 +527,12 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   else
   {
     // Error
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED2_OFF();
 	LED1_OFF();
+#endif
   }
 
   return STMI2C_SubTra_Busy;
@@ -652,16 +669,11 @@ static inline void i2c_event(struct i2c_periph *periph)
   // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
   
+#ifdef I2C_DEBUG_LED
   LED1_ON();
   LED1_OFF();
+#endif
 
-/*
-  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_SB, regs->SR1) )
-  {
-    LED2_ON();
-    LED2_OFF();
-  }
-*/
 
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -678,6 +690,7 @@ static inline void i2c_event(struct i2c_periph *periph)
     periph->errors->unexpected_event_cnt++;
 
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED2_OFF();
@@ -685,6 +698,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
     // no transaction and also an error?
     LED_SHOW_ACTIVE_BITS(regs->SR1);
+#endif
 
     stmi2c_clear_pending_interrupts(regs);
     i2c_error(periph);
@@ -703,14 +717,12 @@ static inline void i2c_event(struct i2c_periph *periph)
     // Close the Bus
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
 
-
+#ifdef I2C_DEBUG_LED
         LED1_ON();
         LED2_ON();
 	LED1_OFF();
 	LED2_OFF();
-
-    //uint16_t SR2 __attribute__ ((unused)) = regs->SR2;	// Clear ADDR
-    //regs->DR = 0x00;					// Silent BTF, Clear Start, or keep provinding SCL in case of unfinished Read
+#endif
 
     // Prepare for next
     ret = STMI2C_SubTra_Ready;
@@ -762,10 +774,12 @@ static inline void i2c_event(struct i2c_periph *periph)
       if (ret == STMI2C_SubTra_Ready)
       {
         // Program a stop
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
         // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
         regs->CR1 |= I2C_CR1_BIT_STOP;
 
@@ -784,12 +798,14 @@ static inline void i2c_event(struct i2c_periph *periph)
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
         periph->status = I2CIdle;
+#ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
+#endif
       }
       // if not, start next transaction
       else
@@ -855,7 +871,9 @@ static inline void i2c_error(struct i2c_periph *periph)
     err_nr = 7;
   }
 
+#ifdef I2C_DEBUG_LED
   LED_ERROR(20, err_nr);
+#endif
 
   return;
   
@@ -892,7 +910,9 @@ void i2c1_hw_init(void) {
   ZEROS_ERR_COUNTER(i2c1_errors);
 
   // Extra
+#ifdef I2C_DEBUG_LED
   LED_INIT();
+#endif
 }
 
 void i2c1_ev_irq_handler(void) {
@@ -959,14 +979,13 @@ void i2c2_hw_init(void) {
 
   I2C_Init(I2C2, i2c2.init_struct);
 
-//  I2C_SoftwareResetCmd(I2C2, ENABLE);
-//  I2C_SoftwareResetCmd(I2C2, DISABLE);
-
   // enable error interrupts
   I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
 
 }
 
+
+// TODO: this was a testing routine
 void i2c2_setbitrate(int bitrate)
 {
   I2C2_InitStruct.I2C_ClockSpeed = bitrate;
@@ -1009,6 +1028,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
   // if (PPRZ_I2C_IS_IDLE(p))
   if (periph->status == I2CIdle)
   {
+	// TODO: re-enable I2C1
 	if (periph == &i2c1)
 	{
 /*

From 5e443f77ca12dd72458f2c5a6972763538efd5bc Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 10:15:51 +0200
Subject: [PATCH 36/74] Import selected files from stm_i2c

---
 conf/autopilot/rotorcraft.makefile            |    3 +-
 .../subsystems/fixedwing/autopilot.makefile   |    5 +-
 .../subsystems/shared/i2c_select.makefile     |   11 +
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 1064 +++++++++++++++++
 4 files changed, 1078 insertions(+), 5 deletions(-)
 create mode 100644 conf/autopilot/subsystems/shared/i2c_select.makefile
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c

diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
index 27a2e084cf..6f621b6dd5 100644
--- a/conf/autopilot/rotorcraft.makefile
+++ b/conf/autopilot/rotorcraft.makefile
@@ -101,8 +101,7 @@ ap.srcs += mcu_periph/uart.c
 ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 # I2C is needed for speed controllers and barometers on lisa
-ap.srcs += mcu_periph/i2c.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
+include subsystems/shared/i2c_select.makefile
 
 ap.srcs += $(SRC_FIRMWARE)/commands.c
 
diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index e5b985c325..7538407364 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -87,9 +87,8 @@ $(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_
 #
 # I2C
 #
-$(TARGET).srcs += mcu_periph/i2c.c
-$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
-
+include subsystems/shared/i2c_select.makefile
+	
 ######################################################################
 ##
 ## COMMON FOR ALL NON-SIMULATION TARGETS
diff --git a/conf/autopilot/subsystems/shared/i2c_select.makefile b/conf/autopilot/subsystems/shared/i2c_select.makefile
new file mode 100644
index 0000000000..0a023a1e43
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/i2c_select.makefile
@@ -0,0 +1,11 @@
+#generic i2c driver
+
+# TODO: this file was created to be able to select different driver files. Once 1 driver is selected as the best others can be removed including this file
+
+$(TARGET).srcs += mcu_periph/i2c.c
+ifeq ($(ARCH), stm32)
+$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.rewritten.c
+else
+$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
+endif
+
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
new file mode 100644
index 0000000000..25337d747b
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -0,0 +1,1064 @@
+#include "mcu_periph/i2c.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+
+//#include "led.h" 
+
+//#define I2C_DEBUG_LED
+
+/////////// DEBUGGING //////////////
+// TODO: remove this 
+
+
+#ifdef I2C_DEBUG_LED
+static inline void LED1_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
+}
+
+static inline void LED1_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
+}
+
+static inline void LED2_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
+}
+
+static inline void LED2_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET );
+}
+
+static inline void LED_INIT(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  LED1_OFF();
+  LED2_OFF();
+}
+
+
+static inline void LED_ERROR(uint8_t base, uint8_t nr)
+{
+  LED2_ON();
+  for (int i=0;i<(base+nr);i++)
+  {
+    LED1_ON();
+    LED1_OFF();    
+  }
+  LED2_OFF();
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef  I2C1_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 200000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef  I2C2_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+//      .I2C_ClockSpeed = 37000
+      .I2C_ClockSpeed = 400000
+};
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// Bypassing the libSTM I2C functions to have more control over the reading of registers
+// e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
+
+// Referring to STM32 manual: 
+// -Doc ID 13902 Rev 11
+
+// Status Register 1
+
+#define I2C_SR1_BIT_SB			(1<<0)		// Start Condition Met
+#define I2C_SR1_BIT_ADDR		(1<<1)		// Address Sent
+#define I2C_SR1_BIT_BTF			(1<<2)		// SCL held low
+#define I2C_SR1_BIT_RXNE		(1<<6)		// Data Read Available
+#define I2C_SR1_BIT_TXE			(1<<7)		// TX buffer space available
+
+#define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop (usually interference)
+#define I2C_SR1_BIT_ERR_ARLO		(1<<9)		// Arbitration Lost (in multimaster) or SDA short-to-ground (in single master)
+#define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure (too fast/too soon/no sensor/wiring break/...)
+#define I2C_SR1_BIT_ERR_OVR		(1<<11)		// Overrun [data loss] (in slave) or SCL short-to-ground (in single master)
+
+#define I2C_SR1_BITS_ERR		((1<<8)|(1<<9)|(1<<10)|(1<<11)|(1<<12)|(1<<14)|(1<<15))
+
+// Status Register 2
+
+#define I2C_SR2_BIT_TRA			(1<<2)		// Transmitting
+#define I2C_SR2_BIT_BUSY		(1<<1)		// Busy
+#define I2C_SR2_BIT_MSL			(1<<0)		// Master Selected
+
+// Control Register 1
+
+#define I2C_CR1_BIT_PE			(1<<0)		// Peripheral Enable
+#define I2C_CR1_BIT_START		(1<<8)		// Generate a Start
+#define I2C_CR1_BIT_STOP		(1<<9)		// Generate a Stop
+#define I2C_CR1_BIT_ACK			(1<<10)		// ACK / NACK
+#define I2C_CR1_BIT_POS			(1<<11)		// Ack will control not the next but secondnext received byte
+#define I2C_CR1_BIT_SWRST		(1<<15)		// Clear Busy Condition when no stop was detected
+
+// Control Register 2
+
+#define I2C_CR2_BIT_ITERREN		(1<<8)		// Error Interrupt
+#define I2C_CR2_BIT_ITEVTEN		(1<<9)		// Event Interrupt
+#define I2C_CR2_BIT_ITBUFEN		(1<<10)		// Buffer Interrupt
+
+
+// Bit Control
+
+#define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// (RE)START
+
+static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  periph->idx_buf = 0;
+  periph->status = I2CStartRequested;
+
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
+  {
+    regs->CR1 &= ~ I2C_CR1_BIT_STOP;
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+  }
+  else
+  {
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+  }
+
+  // Issue a new start
+  regs->CR1 |=  I2C_CR1_BIT_START;
+  
+  // Enable Error IRQ, Event IRQ but disable Buffer IRQ
+  regs->CR2 |= I2C_CR2_BIT_ITERREN;
+  regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+}
+
+// TODO: remove debug
+#ifdef I2C_DEBUG_LED
+
+static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
+{
+  LED1_ON();
+
+  // Start
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // Addr
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // BTF
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // ERROR
+  if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+
+  LED1_OFF();
+}
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+//	SUBTRANSACTION SEQUENCES
+
+enum STMI2CSubTransactionStatus {
+  STMI2C_SubTra_Busy,
+  STMI2C_SubTra_Ready_StopRequested,
+  STMI2C_SubTra_Ready
+};
+
+static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // Disable buffer interrupt
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Send Slave address and wait for ADDR interrupt
+    regs->DR = trans->slave_addr;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // Now read SR2 to clear the ADDR
+    uint16_t SR2  __attribute__ ((unused)) = regs->SR2;
+
+    // Maybe check we are transmitting (did not loose arbitration for instance)
+    // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
+
+    // Send First max 2 bytes
+    regs->DR = trans->buf[0];
+    if (trans->len_w > 1)
+    {
+      regs->DR = trans->buf[1];
+      periph->idx_buf = 2;
+    }
+    else
+    {
+      periph->idx_buf = 1;
+    }
+
+    // Enable buffer-space available interrupt
+    // only if there is more to send: wait for TXE, no more to send: wait for BTF
+    if ( periph->idx_buf < trans->len_w)
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  // The buffer is not full anymore AND we were not waiting for BTF
+  else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
+  {
+    // Send the next byte
+    regs->DR = trans->buf[periph->idx_buf];
+    periph->idx_buf++;
+
+    // All bytes Sent? Then wait for BTF instead
+    if ( periph->idx_buf >= trans->len_w)
+    {
+      // Not interested anymore to know the buffer has space left
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+      // Next interrupt will be BTF (or error)
+    }
+  }
+  // BTF: means last byte was sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+      if (trans->type == I2CTransTx)
+      {
+        // Tell the driver we are ready
+        trans->status = I2CTransSuccess;
+      }
+
+      return STMI2C_SubTra_Ready;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // First Clear the ACK bit: after the next byte we do not want new bytes
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // --- next to steps MUST be executed together to avoid missing the stop
+    __disable_irq();
+
+    // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+      
+    // Schedule a Stop
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+
+    __enable_irq();
+    // --- end of critical zone -----------
+
+#ifdef I2C_DEBUG_LED
+        // Program a stop
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+
+    // Enable the RXNE to get the result
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    trans->buf[0] = regs->DR;
+
+    // We got all the results (stop condition might still be in progress but this is the last interrupt)
+    trans->status = I2CTransSuccess;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    regs->CR1 |= I2C_CR1_BIT_POS;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    // clearing ACK after the byte transfer has already started will NACK the next (2nd)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+      
+    // --- make absolutely sure this command is not delayed too much after the previous: 
+    __disable_irq();
+    //       if transfer of DR was finished already then we will get too many bytes
+    // NOT First Clear the ACK bit but only AFTER clearing ADDR
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Disable the RXNE and wait for BTF
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    __enable_irq();
+    // --- end of critical zone -----------
+  }
+  // Receive buffer if full, master is halted: BTF
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Stop condition MUST be set BEFORE reading the DR
+    // otherwise since there is new buffer space a new byte will be read
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+        // Program a stop
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+
+    trans->buf[0] = regs->DR;
+    trans->buf[1] = regs->DR;
+
+    // We got all the results
+    trans->status = I2CTransSuccess;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // The first data byte will be acked in read many so the slave knows it should send more
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    // Clear the SB flag
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    periph->idx_buf = 0;
+
+    // Enable RXNE: receive an interrupt any time a byte is available
+    // only enable if MORE than 3 bytes need to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+
+    // ACK is still on to get more DATA
+    // Read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // one or more bytes are available AND we were interested in Buffer interrupts
+  else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
+  {
+    // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+    }
+    // from : 3bytes -> last byte: do nothing
+    //
+    // finally: this was the last byte
+    else if (periph->idx_buf >= (trans->len_r - 1))
+    {
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+      // Last Value
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+
+      // We got all the results
+      trans->status = I2CTransSuccess;
+
+      return STMI2C_SubTra_Ready_StopRequested;
+    }
+
+    // Check for end of transaction: start waiting for BTF instead of RXNE
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+    else // idx >= len-3: there are 3 bytes to be read
+    {
+      // We want to halt I2C to have sufficient time to clear ACK, so:
+      // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer
+      // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted)
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    }
+  }
+  // Buffer is full while this was not a RXNE interrupt
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Now the shift register and data register contain data(n-2) and data(n-1)
+    // And I2C is halted so we have time
+
+    // --- Make absolutely sure the next 2 I2C actions are performed with no delay
+    __disable_irq();
+
+    // First we clear the ACK while the SCL is held low by BTF
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+
+    // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
+    regs->CR1 |= I2C_CR1_BIT_STOP;
+        // Program a stop
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+
+    __enable_irq();
+    // --- end of critical zone -----------
+
+    // read the byte2 we had in the buffer (BTF means 2 bytes available)
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+    
+    // Ask for an interrupt to read the last byte (which is normally still busy now)
+    // The last byte will be received with RXNE
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else
+  {
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+#endif
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+
+static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
+{
+  uint16_t SR1 = regs->SR1;
+
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+  // Start Condition Was Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // SB: cleared by software when reading SR1 and writing to DR
+    regs->DR = 0x00;
+  }
+  // Address Was Sent
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // ADDR: Cleared by software when reading SR1 and then SR2
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // Byte Transfer Finished
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // SB: cleared by software when reading SR1 and reading/writing to DR
+    uint8_t dummy __attribute__ ((unused)) = regs->DR;
+    regs->DR = 0x00;
+  }
+}
+
+
+static inline void i2c_error(struct i2c_periph *periph);
+
+static inline void i2c_event(struct i2c_periph *periph)
+{
+
+  /*	
+	There are 7 possible reasons to get here:
+
+	If IT_EV_FEN
+	-------------------------
+
+	We are always interested in all IT_EV_FEV: all are required.
+
+	1) SB		// Start Condition Success in Master mode
+	2) ADDR		// Address sent received Acknoledge
+	[3 ADDR10]	// -- 10bit address stuff
+	[4 STOPF]	// -- only for slaves: master has no stop interrupt
+	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
+
+	// Beware: using the buffered I2C has some interesting properties:
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
+           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
+	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
+	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+        
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and 
+           a new start in one go. 
+
+          -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
+           This also means you must think more in advance and a transaction could be popped from the stack even before it is
+           actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
+           is known.
+	    
+	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
+
+	If IT_EV_FEN AND IT_EV_BUF
+	--------------------------
+
+	Buffer event are not always wanted and are tipically switched on during longer data transfers. Make sure to turn off in time.
+
+	6) RxNE
+	7) TxE
+
+	--------------------------------------------------------------------------------------------------
+	// This driver uses only a subset of the pprz_i2c_states for several reasons:
+	// -we have less interrupts than the I2CStatus states (for efficiency)
+	// -STM32 has such a powerfull I2C engine with plenty of status register flags that
+            only little extra status information needs to be stored.
+
+       // Status is re-used (abused) to remember the last COMMAND THAT WAS SENT to the STM I2C hardware.
+
+// TODO: check which are used
+	enum I2CStatus {
+	  I2CIdle,			// No more last command
+
+	  I2CStartRequested,		// Last command was start
+	  I2CRestartRequested,		// Last command was restart
+	  I2CStopRequested,		// Very important to not send double stop conditions
+
+	  I2CSendingByte,		// Some address/data operation
+
+	  // Following are not used
+	  I2CReadingByte,
+	  I2CAddrWrSent,	
+	  I2CAddrRdSent,
+	  I2CSendingLastByte,
+	  I2CReadingLastByte,
+	  I2CComplete,
+	  I2CFailed
+	};
+
+	---------
+
+	The STM waits indefinately (holding SCL low) for user interaction:
+	a) after a master-start (waiting for address)
+	b) after an address (waiting for data)
+	   not during data sending when using buffered
+	c) after the last byte is transmitted (waiting for either stop or restart)
+	   not during data receiving when using buffered
+	   not after the last byte is received
+
+	-The STM I2C stalls indefinately when a stop condition was attempted that
+	did not succeed. The BUSY flag remains on.
+        -There is no STOP interrupt: use needs another way to finish.
+
+   */
+
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  // Reading the status:
+  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
+  //   the transmission of the byte is finished. At higher clock rates that can be
+  //   quite fast: so we allow no other interrupt to be triggered in between
+  //   reading the status and setting all needed flags
+
+  // Direct Access to the I2C Registers
+  // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+  
+#ifdef I2C_DEBUG_LED
+  LED1_ON();
+  LED1_OFF();
+#endif
+
+
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  //
+  //	TRANSACTION HANDLER
+
+  enum STMI2CSubTransactionStatus ret = 0;
+  uint8_t restart = 0;
+
+  // Nothing Left To Do
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    periph->status = I2CIdle;
+    periph->errors->unexpected_event_cnt++;
+
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+
+    // no transaction and also an error?
+    LED_SHOW_ACTIVE_BITS(regs->SR1);
+#endif
+
+    stmi2c_clear_pending_interrupts(regs);
+    i2c_error(periph);
+
+    // There are no transactions anymore: so we are not allowed to continue
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+
+  if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+  {
+    // Set result in transaction
+    trans->status = I2CTransFailed;
+
+    // Close the Bus
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
+
+#ifdef I2C_DEBUG_LED
+        LED1_ON();
+        LED2_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+
+    // Prepare for next
+    ret = STMI2C_SubTra_Ready;
+    restart = 0;
+
+    stmi2c_clear_pending_interrupts(regs);
+
+    // Count it
+    i2c_error(periph);
+  }
+  else
+  {
+
+    if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
+    {
+      switch (trans->len_r)
+      {
+        case 1:
+          ret = stmi2c_read1(regs,trans);
+          break;
+        case 2:
+          ret = stmi2c_read2(regs,trans);
+          break;
+        default:
+          ret = stmi2c_readmany(regs,periph, trans);
+          break;    
+      }
+    }
+    else // TxRx or Tx
+    {
+      ret = stmi2c_send(regs,periph,trans);
+      if (trans->type == I2CTransTxRx)
+      {
+        restart = 1;
+      }
+    }
+  }
+
+  // Sub-transaction not finished
+  if (ret == STMI2C_SubTra_Busy)
+  {
+    // Remember the last command was not start or stop
+    periph->status = I2CSendingByte;
+  }
+  else // Finished?
+  {
+    if (restart == 0)
+    {
+      if (ret == STMI2C_SubTra_Ready)
+      {
+        // Program a stop
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+        // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+        regs->CR1 |= I2C_CR1_BIT_STOP;
+
+        // Silent any BTF that would occur before STOP is executed
+        regs->DR = 0x00;
+
+        periph->status = I2CStopRequested;
+      }
+
+      // Jump to the next transaction
+      periph->trans_extract_idx++;
+      if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+        periph->trans_extract_idx = 0;
+
+      // if we have no more transaction to process, stop here 
+      if (periph->trans_extract_idx == periph->trans_insert_idx)
+      {
+        periph->status = I2CIdle;
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+      }
+      // if not, start next transaction
+      else
+      {
+        // Restart transaction doing the Rx part now
+        periph->status = I2CStartRequested;
+        PPRZ_I2C_SEND_START(periph);
+     }
+
+    }
+    // RxTx -> Restart and do Rx part
+    else 
+    {
+      trans->type = I2CTransRx;
+      periph->status = I2CStartRequested;
+      regs->CR1 |= I2C_CR1_BIT_START;
+
+      // Silent any BTF that would occur before SB
+      regs->DR = 0x00;
+    }
+  }
+
+  return;
+}
+
+static inline void i2c_error(struct i2c_periph *periph)
+{
+  uint8_t err_nr = 0;
+  periph->errors->er_irq_cnt;
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
+    periph->errors->ack_fail_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_AF);
+    err_nr = 1;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
+    periph->errors->miss_start_stop_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BERR);
+    err_nr = 2;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
+    periph->errors->arb_lost_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_ARLO);
+    err_nr = 3;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
+    periph->errors->over_under_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_OVR);
+    err_nr = 4;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
+    periph->errors->pec_recep_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_PECERR);
+    err_nr = 5;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
+    periph->errors->timeout_tlow_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_TIMEOUT);
+    err_nr = 6;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+    periph->errors->smbus_alert_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_SMBALERT);
+    err_nr = 7;
+  }
+
+#ifdef I2C_DEBUG_LED
+  LED_ERROR(20, err_nr);
+#endif
+
+  return;
+  
+}
+
+  
+/*
+  // Make sure the bus is free before resetting (p722)
+  if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
+    // Reset the I2C block
+    I2C_SoftwareResetCmd(periph->reg_addr, ENABLE);
+    I2C_SoftwareResetCmd(periph->reg_addr, DISABLE);
+  }
+*/
+
+//#endif /* USE_I2C2 */
+
+
+
+
+#ifdef USE_I2C1
+
+struct i2c_errors i2c1_errors;
+
+void i2c1_hw_init(void) {
+
+  i2c1.reg_addr = I2C1;
+  i2c1.init_struct = &I2C1_InitStruct;
+  i2c1.scl_pin = GPIO_Pin_6;
+  i2c1.sda_pin = GPIO_Pin_7;
+  i2c1.errors = &i2c1_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c1_errors);
+
+  // Extra
+#ifdef I2C_DEBUG_LED
+  LED_INIT();
+#endif
+}
+
+void i2c1_ev_irq_handler(void) {
+  i2c_event(&i2c1);
+}
+
+void i2c1_er_irq_handler(void) {
+  i2c_event(&i2c1);
+}
+
+#endif /* USE_I2C1 */
+
+#ifdef USE_I2C2
+
+struct i2c_errors i2c2_errors;
+
+void i2c2_hw_init(void) {
+
+  i2c2.reg_addr = I2C2;
+  i2c2.init_struct = &I2C2_InitStruct;
+  i2c2.scl_pin = GPIO_Pin_10;
+  i2c2.sda_pin = GPIO_Pin_11;
+  i2c2.errors = &i2c2_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c2_errors);
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C2);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C2 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C2 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C2 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c2.scl_pin | i2c2.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C2);
+
+  // enable peripheral
+  I2C_Cmd(I2C2, ENABLE);
+
+  I2C_Init(I2C2, i2c2.init_struct);
+
+  // enable error interrupts
+  I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
+
+}
+
+
+// TODO: this was a testing routine
+void i2c2_setbitrate(int bitrate)
+{
+  I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+  I2C_Init(I2C2, i2c2.init_struct);
+}
+
+
+
+void i2c2_ev_irq_handler(void) {
+  i2c_event(&i2c2);
+}
+
+void i2c2_er_irq_handler(void) {
+  i2c_event(&i2c2);
+}
+
+#endif /* USE_I2C2 */
+
+
+
+/////////////////////////////////////////////////////////
+// Implement Interface Functions
+
+bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
+
+  uint8_t temp;
+  temp = periph->trans_insert_idx + 1;
+  if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
+  if (temp == periph->trans_extract_idx)
+    return FALSE;                          // queue full
+
+  t->status = I2CTransPending;
+
+  __disable_irq();
+  /* put transacation in queue */
+  periph->trans[periph->trans_insert_idx] = t;
+  periph->trans_insert_idx = temp;
+
+  /* if peripheral is idle, start the transaction */
+  // if (PPRZ_I2C_IS_IDLE(p))
+  if (periph->status == I2CIdle)
+  {
+	// TODO: re-enable I2C1
+	if (periph == &i2c1)
+	{
+/*
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+*/
+        }
+        else
+        {
+          PPRZ_I2C_SEND_START(periph);
+        }
+  }
+  /* else it will be started by the interrupt handler when the previous transactions completes */
+  __enable_irq();
+
+  return TRUE;
+}
+
+bool_t i2c_idle(struct i2c_periph* periph)
+{
+  return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
+  //return periph->status == I2CIdle;
+}
+
+

From c4f02de0930ee90a582a4074df1d871414355457 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 10:31:58 +0200
Subject: [PATCH 37/74] test1: subsystem name=aspirin_i2c.xml works

---
 conf/autopilot/rotorcraft.makefile                     | 2 +-
 conf/autopilot/subsystems/fixedwing/autopilot.makefile | 2 +-
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c | 4 ++--
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
index 6f621b6dd5..11af20574b 100644
--- a/conf/autopilot/rotorcraft.makefile
+++ b/conf/autopilot/rotorcraft.makefile
@@ -101,7 +101,7 @@ ap.srcs += mcu_periph/uart.c
 ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 # I2C is needed for speed controllers and barometers on lisa
-include subsystems/shared/i2c_select.makefile
+include $(CFG_SHARED)/i2c_select.makefile
 
 ap.srcs += $(SRC_FIRMWARE)/commands.c
 
diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 7538407364..11c6dd7b82 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -87,7 +87,7 @@ $(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_
 #
 # I2C
 #
-include subsystems/shared/i2c_select.makefile
+include $(CFG_SHARED)/i2c_select.makefile
 	
 ######################################################################
 ##
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 25337d747b..5f7b6b9648 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1029,7 +1029,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
   if (periph->status == I2CIdle)
   {
 	// TODO: re-enable I2C1
-	if (periph == &i2c1)
+//	if (periph == &i2c1)
 	{
 /*
         LED2_ON();
@@ -1044,7 +1044,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 	LED2_OFF();
 */
         }
-        else
+//        else
         {
           PPRZ_I2C_SEND_START(periph);
         }

From 0f5792388aa06ef330f3d2c863715c649efc1209 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 12:04:34 +0200
Subject: [PATCH 38/74] Default baudrate set same as LPC21: should give WAY
 less trouble on long I2C lines

---
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 5f7b6b9648..fa10e246e7 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -82,7 +82,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
 //      .I2C_ClockSpeed = 37000
-      .I2C_ClockSpeed = 400000
+      .I2C_ClockSpeed = 40000
 };
 #endif
 

From 6988843baf7ff7147521008c4c137997e42ee14e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 12:05:31 +0200
Subject: [PATCH 39/74] @Allen: I have an idea why you added this but it should
 not be needed anymore

---
 sw/airborne/boards/lisa_l/baro_board.c | 2 --
 1 file changed, 2 deletions(-)

diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c
index 11829b5acf..d657d19faf 100644
--- a/sw/airborne/boards/lisa_l/baro_board.c
+++ b/sw/airborne/boards/lisa_l/baro_board.c
@@ -25,8 +25,6 @@ void baro_init(void) {
 
 
 void baro_periodic(void) {
-  // check i2c_done
-  if (!i2c_idle(&i2c2)) return;
   switch (baro_board.status) {
   case LBS_UNINITIALIZED:
     baro_board_send_reset();

From 3781fd736cfa2dbebac2f84ef4f5e2dc60d58c80 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 14:53:02 +0200
Subject: [PATCH 40/74] Commenting and cleaner TxRx handling

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 68 +++++++++++--------
 1 file changed, 41 insertions(+), 27 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index fa10e246e7..b70051ba6d 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -70,7 +70,7 @@ static I2C_InitTypeDef  I2C1_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 200000
+      .I2C_ClockSpeed = 400000
 };
 #endif
 
@@ -543,7 +543,8 @@ static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
 {
   uint16_t SR1 = regs->SR1;
 
-  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
+
 
   // Start Condition Was Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
@@ -681,15 +682,11 @@ static inline void i2c_event(struct i2c_periph *periph)
   //	TRANSACTION HANDLER
 
   enum STMI2CSubTransactionStatus ret = 0;
-  uint8_t restart = 0;
 
+  ///////////////////////
   // Nothing Left To Do
   if (periph->trans_extract_idx == periph->trans_insert_idx)
   {
-    periph->status = I2CIdle;
-    periph->errors->unexpected_event_cnt++;
-
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
 #ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();
@@ -700,22 +697,32 @@ static inline void i2c_event(struct i2c_periph *periph)
     LED_SHOW_ACTIVE_BITS(regs->SR1);
 #endif
 
+    // If we still get an interrupt but there are no more things to do
+    // (which can happen if an event was sheduled just before a bus error occurs)
+    // then its easy: just stop: clear all interrupt generating bits  
+
+    // Clear Running Events
     stmi2c_clear_pending_interrupts(regs);
+
+    // Count The Errors
     i2c_error(periph);
 
-    // There are no transactions anymore: so we are not allowed to continue
+    // Mark this as a special error
+    periph->errors->unexpected_event_cnt++;
+
+    periph->status = I2CIdle;
+
+    // There are no transactions anymore: 
+    // furtheron we need a transaction pointer: so we are not allowed to continue
     return;
   }
 
   struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
 
+  ///////////////////////////
+  // If there was an error:
   if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
   {
-    // Set result in transaction
-    trans->status = I2CTransFailed;
-
-    // Close the Bus
-    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE RXNE
 
 #ifdef I2C_DEBUG_LED
         LED1_ON();
@@ -724,15 +731,25 @@ static inline void i2c_event(struct i2c_periph *periph)
 	LED2_OFF();
 #endif
 
+    // Set result in transaction
+    trans->status = I2CTransFailed;
+
     // Prepare for next
     ret = STMI2C_SubTra_Ready;
-    restart = 0;
 
+    // Make sure a TxRx does not Restart
+    trans->type == I2CTransRx;
+
+    // Clear Running Events
     stmi2c_clear_pending_interrupts(regs);
 
-    // Count it
+    // Count The Errors
     i2c_error(periph);
   }
+
+
+  ///////////////////////////
+  // Normal Event:
   else
   {
 
@@ -754,25 +771,20 @@ static inline void i2c_event(struct i2c_periph *periph)
     else // TxRx or Tx
     {
       ret = stmi2c_send(regs,periph,trans);
-      if (trans->type == I2CTransTxRx)
-      {
-        restart = 1;
-      }
     }
   }
 
-  // Sub-transaction not finished
-  if (ret == STMI2C_SubTra_Busy)
+  /////////////////////////////////
+  // Sub-transaction has finished
+  if (ret != STMI2C_SubTra_Busy)
   {
-    // Remember the last command was not start or stop
-    periph->status = I2CSendingByte;
-  }
-  else // Finished?
-  {
-    if (restart == 0)
+    // If a restart is not needed
+    if (trans->type != I2CTransTxRx)
     {
+      // Ready, no stop condition set yet
       if (ret == STMI2C_SubTra_Ready)
       {
+
         // Program a stop
 #ifdef I2C_DEBUG_LED
         LED2_ON();
@@ -780,6 +792,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 	LED1_OFF();
 	LED2_OFF();
 #endif
+
         // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
         regs->CR1 |= I2C_CR1_BIT_STOP;
 
@@ -798,6 +811,7 @@ static inline void i2c_event(struct i2c_periph *periph)
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
         periph->status = I2CIdle;
+
 #ifdef I2C_DEBUG_LED
         LED2_ON();
         LED1_ON();

From d65e190c596a9bc865c5dfa1eca16f1f214ffd81 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 15:38:55 +0200
Subject: [PATCH 41/74] I know... its horrible. I'm so sorry. That is the only
 way I know to comply with the datasheet requirements. :'-(

---
 sw/airborne/arch/avr/mcu_periph/i2c_arch.c    |  4 +
 sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c  |  5 +
 sw/airborne/arch/sim/mcu_periph/i2c_arch.c    |  3 +
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 92 +++++++++++++++++--
 sw/airborne/firmwares/fixedwing/main_ap.c     |  4 +
 sw/airborne/firmwares/fixedwing/main_fbw.c    |  2 +
 sw/airborne/firmwares/rotorcraft/main.c       |  2 +
 sw/airborne/mcu_periph/i2c.h                  |  3 +
 8 files changed, 107 insertions(+), 8 deletions(-)

diff --git a/sw/airborne/arch/avr/mcu_periph/i2c_arch.c b/sw/airborne/arch/avr/mcu_periph/i2c_arch.c
index f427a64008..c3953dfb3a 100644
--- a/sw/airborne/arch/avr/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/avr/mcu_periph/i2c_arch.c
@@ -161,3 +161,7 @@ void i2c_init(void) {
   sbi(TWSR, TWPS0);
   */
 }
+
+void i2c_event(void) { }
+void i2c2_setbitrate(int bitrate __attribute__ ((unused))) { }
+
diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index 98f65140a9..49c46ed1ab 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -340,3 +340,8 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
 
   return TRUE;
 }
+
+void i2c_event(void) { }
+void i2c2_setbitrate(int bitrate __attribute__ ((unused))) { }
+
+
diff --git a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c
index ceb58123b6..34b43d9739 100644
--- a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c
@@ -4,3 +4,6 @@ void i2c_hw_init ( void ) {}
 
 bool_t i2c_idle(struct i2c_periph *p __attribute__ ((unused))) { return TRUE; }
 bool_t i2c_submit(struct i2c_periph* p __attribute__ ((unused)), struct i2c_transaction* t __attribute__ ((unused))) { return TRUE;}
+void i2c_event(void) { }
+void i2c2_setbitrate(int bitrate __attribute__ ((unused))) { }
+
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index b70051ba6d..e150121eb3 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -570,7 +570,7 @@ static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
 
 static inline void i2c_error(struct i2c_periph *periph);
 
-static inline void i2c_event(struct i2c_periph *periph)
+static inline void i2c_irq(struct i2c_periph *periph)
 {
 
   /*	
@@ -738,7 +738,7 @@ static inline void i2c_event(struct i2c_periph *periph)
     ret = STMI2C_SubTra_Ready;
 
     // Make sure a TxRx does not Restart
-    trans->type == I2CTransRx;
+    trans->type = I2CTransRx;
 
     // Clear Running Events
     stmi2c_clear_pending_interrupts(regs);
@@ -825,8 +825,11 @@ static inline void i2c_event(struct i2c_periph *periph)
       else
       {
         // Restart transaction doing the Rx part now
-        periph->status = I2CStartRequested;
-        PPRZ_I2C_SEND_START(periph);
+        
+// --- moved to idle function
+//        periph->status = I2CStartRequested;
+//        PPRZ_I2C_SEND_START(periph);
+// ------
      }
 
     }
@@ -926,15 +929,59 @@ void i2c1_hw_init(void) {
   // Extra
 #ifdef I2C_DEBUG_LED
   LED_INIT();
+#else
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C1);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C1 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C1 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C1 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c1.scl_pin | i2c1.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C1);
+
+  // enable peripheral
+  I2C_Cmd(I2C1, ENABLE);
+
+  I2C_Init(I2C1, i2c1.init_struct);
+
+  // enable error interrupts
+  I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE);
+
 #endif
 }
 
 void i2c1_ev_irq_handler(void) {
-  i2c_event(&i2c1);
+  i2c_irq(&i2c1);
 }
 
 void i2c1_er_irq_handler(void) {
-  i2c_event(&i2c1);
+  i2c_irq(&i2c1);
 }
 
 #endif /* USE_I2C1 */
@@ -1009,16 +1056,45 @@ void i2c2_setbitrate(int bitrate)
 
 
 void i2c2_ev_irq_handler(void) {
-  i2c_event(&i2c2);
+  i2c_irq(&i2c2);
 }
 
 void i2c2_er_irq_handler(void) {
-  i2c_event(&i2c2);
+  i2c_irq(&i2c2);
 }
 
 #endif /* USE_I2C2 */
 
 
+void i2c_event(void) 
+{
+#ifdef USE_I2C1
+  if (i2c_idle(&i2c1))
+  {
+    __disable_irq();
+    // More work to do
+    if (i2c1.trans_extract_idx != i2c1.trans_insert_idx)
+    {
+      // Restart transaction doing the Rx part now
+      PPRZ_I2C_SEND_START(&i2c1);
+    }
+    __disable_irq();
+  }
+#endif
+#ifdef USE_I2C2
+  if (i2c_idle(&i2c2))
+  {
+    __disable_irq();
+    // More work to do
+    if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
+    {
+      // Restart transaction doing the Rx part now
+      PPRZ_I2C_SEND_START(&i2c2);
+    }
+    __disable_irq();
+  }
+#endif
+}
 
 /////////////////////////////////////////////////////////
 // Implement Interface Functions
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 7c26ebd2dd..01fe2f9a58 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -615,6 +615,10 @@ void init_ap( void ) {
 /*********** EVENT ***********************************************************/
 void event_task_ap( void ) {
 
+#ifndef SINGLE_MCU
+  i2c_event();
+#endif
+
 #if defined USE_AHRS
 #ifdef USE_IMU
   ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 40dcf65a58..f9f8b6b08b 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -43,6 +43,7 @@
 #include "firmwares/fixedwing/autopilot.h"
 #include "fbw_downlink.h"
 #include "paparazzi.h"
+#include "mcu_periph/i2c.h"
 
 #ifdef MCU_SPI_LINK
 #include "link_mcu.h"
@@ -118,6 +119,7 @@ void event_task_fbw( void) {
   RadioControlEvent(handle_rc_frame);
 #endif
 
+  i2c_event();
 
 #ifdef INTER_MCU
 #ifdef MCU_SPI_LINK
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index b0808d6202..db5e4bb8a0 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -184,6 +184,8 @@ STATIC_INLINE void main_periodic( void ) {
 
 STATIC_INLINE void main_event( void ) {
 
+  i2c_event();
+
   DatalinkEvent();
 
   if (autopilot_rc) {
diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h
index 93bd0f77ba..8d9d3269cb 100644
--- a/sw/airborne/mcu_periph/i2c.h
+++ b/sw/airborne/mcu_periph/i2c.h
@@ -133,6 +133,9 @@ extern void i2c2_init(void);
 extern void   i2c_init(struct i2c_periph* p);
 extern bool_t i2c_idle(struct i2c_periph* p);
 extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t);
+extern void   i2c2_setbitrate(int bitrate);
+extern void   i2c_event(void);
+
 
 #define I2CReceive(_p, _t, _s_addr, _len) { \
   _t.type = I2CTransRx;                     \

From c202ba17335a4244235d499834b8cabe924dc9b0 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 20 Oct 2011 15:42:23 +0200
Subject: [PATCH 42/74] important typo

---
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index e150121eb3..8578867f23 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1078,7 +1078,7 @@ void i2c_event(void)
       // Restart transaction doing the Rx part now
       PPRZ_I2C_SEND_START(&i2c1);
     }
-    __disable_irq();
+    __enable_irq();
   }
 #endif
 #ifdef USE_I2C2
@@ -1091,7 +1091,7 @@ void i2c_event(void)
       // Restart transaction doing the Rx part now
       PPRZ_I2C_SEND_START(&i2c2);
     }
-    __disable_irq();
+    __enable_irq();
   }
 #endif
 }

From a044b5f2f11a875bd14a98672db07ed911b8299b Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 21 Oct 2011 01:58:32 +0200
Subject: [PATCH 43/74] Wait for stop bit cleared by hardware and busy bit
 cleared before next (just as in the datasheet)

---
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 8578867f23..81cc702b97 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -798,8 +798,6 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
         // Silent any BTF that would occur before STOP is executed
         regs->DR = 0x00;
-
-        periph->status = I2CStopRequested;
       }
 
       // Jump to the next transaction
@@ -824,6 +822,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
       // if not, start next transaction
       else
       {
+        periph->status = I2CIdle;
         // Restart transaction doing the Rx part now
         
 // --- moved to idle function
@@ -1116,7 +1115,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
   /* if peripheral is idle, start the transaction */
   // if (PPRZ_I2C_IS_IDLE(p))
-  if (periph->status == I2CIdle)
+  if (i2c_idle(periph))
   {
 	// TODO: re-enable I2C1
 //	if (periph == &i2c1)
@@ -1148,7 +1147,6 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 bool_t i2c_idle(struct i2c_periph* periph)
 {
   return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
-  //return periph->status == I2CIdle;
 }
 
 

From 50b43ebab4219b3b37f73ccc9db842777e1a7efa Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 21 Oct 2011 09:29:28 +0200
Subject: [PATCH 44/74] Restructuring

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 58 +++++++++++--------
 1 file changed, 34 insertions(+), 24 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 81cc702b97..1d2273a468 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -805,10 +805,13 @@ static inline void i2c_irq(struct i2c_periph *periph)
       if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
         periph->trans_extract_idx = 0;
 
+      // Tell everyone we are ready
+      periph->status = I2CIdle;
+
+
       // if we have no more transaction to process, stop here 
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
-        periph->status = I2CIdle;
 
 #ifdef I2C_DEBUG_LED
         LED2_ON();
@@ -822,11 +825,8 @@ static inline void i2c_irq(struct i2c_periph *periph)
       // if not, start next transaction
       else
       {
-        periph->status = I2CIdle;
         // Restart transaction doing the Rx part now
-        
 // --- moved to idle function
-//        periph->status = I2CStartRequested;
 //        PPRZ_I2C_SEND_START(periph);
 // ------
      }
@@ -1068,29 +1068,35 @@ void i2c2_er_irq_handler(void) {
 void i2c_event(void) 
 {
 #ifdef USE_I2C1
-  if (i2c_idle(&i2c1))
+  if (i2c1.status == I2CIdle)
   {
-    __disable_irq();
-    // More work to do
-    if (i2c1.trans_extract_idx != i2c1.trans_insert_idx)
+    if (i2c_idle(&i2c1))
     {
-      // Restart transaction doing the Rx part now
-      PPRZ_I2C_SEND_START(&i2c1);
+      __disable_irq();
+      // More work to do
+      if (i2c1.trans_extract_idx != i2c1.trans_insert_idx)
+      {
+        // Restart transaction doing the Rx part now
+        PPRZ_I2C_SEND_START(&i2c1);
+      }
+      __enable_irq();
     }
-    __enable_irq();
   }
 #endif
 #ifdef USE_I2C2
-  if (i2c_idle(&i2c2))
+  //if (i2c2.status == I2CIdle)
   {
-    __disable_irq();
-    // More work to do
-    if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
+    if (i2c_idle(&i2c2))
     {
-      // Restart transaction doing the Rx part now
-      PPRZ_I2C_SEND_START(&i2c2);
+      __disable_irq();
+      // More work to do
+      if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
+      {
+        // Restart transaction doing the Rx part now
+        PPRZ_I2C_SEND_START(&i2c2);
+      }
+      __enable_irq();
     }
-    __enable_irq();
   }
 #endif
 }
@@ -1115,12 +1121,14 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
   /* if peripheral is idle, start the transaction */
   // if (PPRZ_I2C_IS_IDLE(p))
-  if (i2c_idle(periph))
+  if (periph->status == I2CIdle)
   {
-	// TODO: re-enable I2C1
-//	if (periph == &i2c1)
+    if (i2c_idle(periph))
+    {
+#ifdef I2C_DEBUG_LED
+	if (periph == &i2c1)
 	{
-/*
+
         LED2_ON();
         LED1_ON();
 	LED1_OFF();
@@ -1131,12 +1139,14 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
         LED1_ON();
 	LED1_OFF();
 	LED2_OFF();
-*/
+
         }
-//        else
+        else
+#endif
         {
           PPRZ_I2C_SEND_START(periph);
         }
+    }
   }
   /* else it will be started by the interrupt handler when the previous transactions completes */
   __enable_irq();

From ef1b2df067796b90a3357b5e2adeabfa34d1ef9a Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 21 Oct 2011 10:13:34 +0200
Subject: [PATCH 45/74] More debugging tools

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 199 +++++++++++-------
 1 file changed, 121 insertions(+), 78 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 1d2273a468..4accc48df6 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -70,7 +70,7 @@ static I2C_InitTypeDef  I2C1_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-      .I2C_ClockSpeed = 400000
+      .I2C_ClockSpeed = 40000
 };
 #endif
 
@@ -82,7 +82,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
 //      .I2C_ClockSpeed = 37000
-      .I2C_ClockSpeed = 40000
+      .I2C_ClockSpeed = 400000
 };
 #endif
 
@@ -139,6 +139,104 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
+// TODO: remove debug
+#ifdef I2C_DEBUG_LED
+
+static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
+{
+  uint16_t CR1 = regs->CR1;
+  uint16_t SR1 = regs->SR1;
+  uint16_t SR2 = regs->SR2;
+  // Note: reading SR1 and then SR2 will clear ADDR bits
+
+  LED1_ON();
+
+  // 1 Start
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // 2 Addr
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // 3 BTF
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // 4 ERROR
+  if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+
+  LED1_OFF();
+
+
+  LED1_ON();
+
+  // 1 Start
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_START, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // 2 Stop
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+
+  // 3 Busy
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+
+  // 4 Busy
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+
+  // 5 Busy
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  LED1_OFF();
+  
+}
+#endif
+
+static inline void PPRZ_I2C_SEND_STOP(I2C_TypeDef *regs)
+{
+  // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+  regs->CR1 |= I2C_CR1_BIT_STOP;
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+}
+
 // (RE)START
 
 static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
@@ -148,6 +246,10 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   periph->idx_buf = 0;
   periph->status = I2CStartRequested;
 
+#ifdef I2C_DEBUG_LED
+  LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
   {
     regs->CR1 &= ~ I2C_CR1_BIT_STOP;
@@ -188,45 +290,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
 }
 
-// TODO: remove debug
-#ifdef I2C_DEBUG_LED
-
-static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
-{
-  LED1_ON();
-
-  // Start
-  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
-    LED2_ON();
-  else
-    LED2_OFF();
-  LED2_OFF();    
-  
-  // Addr
-  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
-    LED2_ON();
-  else
-    LED2_OFF();
-  LED2_OFF();    
-  
-  // BTF
-  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
-    LED2_ON();
-  else
-    LED2_OFF();
-  LED2_OFF();    
-  
-  // ERROR
-  if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
-    LED2_ON();
-  else
-    LED2_OFF();
-  LED2_OFF();    
-  
-
-  LED1_OFF();
-}
-#endif
+// STOP
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -331,19 +395,11 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
     // Schedule a Stop
-    regs->CR1 |= I2C_CR1_BIT_STOP;
+    PPRZ_I2C_SEND_STOP(regs);
 
     __enable_irq();
     // --- end of critical zone -----------
 
-#ifdef I2C_DEBUG_LED
-        // Program a stop
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
-
     // Enable the RXNE to get the result
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
@@ -397,14 +453,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
   {
     // Stop condition MUST be set BEFORE reading the DR
     // otherwise since there is new buffer space a new byte will be read
-    regs->CR1 |= I2C_CR1_BIT_STOP;
-        // Program a stop
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
+    PPRZ_I2C_SEND_STOP(regs);
 
     trans->buf[0] = regs->DR;
     trans->buf[1] = regs->DR;
@@ -504,14 +553,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     periph->idx_buf ++;
 
     // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
-    regs->CR1 |= I2C_CR1_BIT_STOP;
-        // Program a stop
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
+    PPRZ_I2C_SEND_STOP(regs);
 
     __enable_irq();
     // --- end of critical zone -----------
@@ -694,7 +736,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 	LED1_OFF();
 
     // no transaction and also an error?
-    LED_SHOW_ACTIVE_BITS(regs->SR1);
+    LED_SHOW_ACTIVE_BITS(regs);
 #endif
 
     // If we still get an interrupt but there are no more things to do
@@ -729,6 +771,8 @@ static inline void i2c_irq(struct i2c_periph *periph)
         LED2_ON();
 	LED1_OFF();
 	LED2_OFF();
+
+        LED_SHOW_ACTIVE_BITS(regs);
 #endif
 
     // Set result in transaction
@@ -786,15 +830,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
       {
 
         // Program a stop
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
-
-        // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
-        regs->CR1 |= I2C_CR1_BIT_STOP;
+        PPRZ_I2C_SEND_STOP(regs);
 
         // Silent any BTF that would occur before STOP is executed
         regs->DR = 0x00;
@@ -1045,7 +1081,6 @@ void i2c2_hw_init(void) {
 }
 
 
-// TODO: this was a testing routine
 void i2c2_setbitrate(int bitrate)
 {
   I2C2_InitStruct.I2C_ClockSpeed = bitrate;
@@ -1067,6 +1102,7 @@ void i2c2_er_irq_handler(void) {
 
 void i2c_event(void) 
 {
+#ifndef I2C_DEBUG_LED
 #ifdef USE_I2C1
   if (i2c1.status == I2CIdle)
   {
@@ -1083,6 +1119,8 @@ void i2c_event(void)
     }
   }
 #endif
+#endif
+
 #ifdef USE_I2C2
   //if (i2c2.status == I2CIdle)
   {
@@ -1156,7 +1194,12 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
 bool_t i2c_idle(struct i2c_periph* periph)
 {
-  return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
+  if (periph->status == I2CIdle)
+  {
+    // Only read status register SR2 when it does not matter to maybe clear ADDR bits
+    return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
+  }
+  return FALSE;
 }
 
 

From d1861195bc60b73583e873920b73e0036f55b068 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 21 Oct 2011 13:06:51 +0200
Subject: [PATCH 46/74] Maior Improvement: We have itentified that a mainloop
 reading the BUSY bit can clear an ADDR interupt. Moreover we have identified
 that when writing a Start ClearStop to CR1 in a single machine code does not
 clear the pending Stop but adds a start not ever folowed by an extra STOP.
 clearing stop then writing start canceled the needed stop, and setting start
 and then clearing stop still sometimes added a second unwanted stop after the
 start the followed the wanted stop. This means the 2 reasons for stopping
 interrupts are suddenly gone...

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 196 ++++++++++++------
 1 file changed, 138 insertions(+), 58 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 4accc48df6..09c7a02dc3 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -7,7 +7,7 @@
 
 //#include "led.h" 
 
-//#define I2C_DEBUG_LED
+#define I2C_DEBUG_LED
 
 /////////// DEBUGGING //////////////
 // TODO: remove this 
@@ -179,6 +179,12 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_OFF();
   LED2_OFF();    
   
+  // Anything?
+  if (( SR1 + SR2) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
 
   LED1_OFF();
 
@@ -206,20 +212,45 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_OFF();
   LED2_OFF();    
 
-  // 4 Busy
+  // 4 Tra
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
   LED2_OFF();    
 
-  // 5 Busy
+  // 5 Master
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
   LED2_OFF();    
   LED1_OFF();
+
+  LED1_ON();
+
+  // 1 Anything CR?
+  if (( CR1) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+
+  // 2 PE
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_PE, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+  
+  // 3 SWRESET
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_SWRST, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();    
+    
+  LED1_OFF();
   
 }
 #endif
@@ -244,50 +275,41 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
   periph->idx_buf = 0;
-  periph->status = I2CStartRequested;
 
 #ifdef I2C_DEBUG_LED
   LED_SHOW_ACTIVE_BITS(regs);
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+
 #endif
 
+/*
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
   {
     regs->CR1 &= ~ I2C_CR1_BIT_STOP;
-
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
-  }
-  else
-  {
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
   }
+*/
 
-  // Issue a new start
-  regs->CR1 |=  I2C_CR1_BIT_START;
-  
   // Enable Error IRQ, Event IRQ but disable Buffer IRQ
   regs->CR2 |= I2C_CR2_BIT_ITERREN;
   regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
   regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+  // Issue a new start
+  regs->CR1 =  (I2C_CR1_BIT_START | I2C_CR1_BIT_PE);
+  periph->status = I2CStartRequested;
+  
+
+#ifdef I2C_DEBUG_LED
+  LED_SHOW_ACTIVE_BITS(regs);
+#endif
 }
 
 // STOP
@@ -863,7 +885,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
       {
         // Restart transaction doing the Rx part now
 // --- moved to idle function
-//        PPRZ_I2C_SEND_START(periph);
+        PPRZ_I2C_SEND_START(periph);
 // ------
      }
 
@@ -1083,8 +1105,28 @@ void i2c2_hw_init(void) {
 
 void i2c2_setbitrate(int bitrate)
 {
-  I2C2_InitStruct.I2C_ClockSpeed = bitrate;
-  I2C_Init(I2C2, i2c2.init_struct);
+  if (i2c_idle(&i2c2))
+  {
+    I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+    I2C_Init(I2C2, i2c2.init_struct);
+  
+
+#ifdef I2C_DEBUG_LED
+        __disable_irq();
+
+        LED2_ON();
+        LED1_ON();
+        LED2_OFF();
+        LED1_OFF();
+        LED2_ON();
+        LED1_ON();
+        LED2_OFF();
+        LED1_OFF();
+
+        __enable_irq();
+#endif
+
+   }
 }
 
 
@@ -1102,6 +1144,11 @@ void i2c2_er_irq_handler(void) {
 
 void i2c_event(void) 
 {
+  static uint32_t cnt = 0;
+  I2C_TypeDef *regs;
+  cnt++;
+  if (cnt > 10000) cnt = 0;
+
 #ifndef I2C_DEBUG_LED
 #ifdef USE_I2C1
   if (i2c1.status == I2CIdle)
@@ -1122,18 +1169,56 @@ void i2c_event(void)
 #endif
 
 #ifdef USE_I2C2
+  
+#ifdef I2C_DEBUG_LED
+  if (cnt == 0)
+  {
+        __disable_irq();
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        if (i2c2.status == I2CIdle)
+        {
+          LED1_ON();
+   	  LED1_OFF();
+        }
+        else if (i2c2.status == I2CStartRequested)
+        {
+          LED1_ON();
+   	  LED1_OFF();
+          LED1_ON();
+   	  LED1_OFF();
+
+        }
+	LED2_OFF();
+
+        //regs = (I2C_TypeDef *) i2c2.reg_addr;
+        //LED_SHOW_ACTIVE_BITS(regs);
+
+        __enable_irq();
+  }
+#endif  
+
+
   //if (i2c2.status == I2CIdle)
   {
-    if (i2c_idle(&i2c2))
+    //if (i2c_idle(&i2c2))
     {
-      __disable_irq();
+      //__disable_irq();
       // More work to do
-      if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
+      //if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
       {
         // Restart transaction doing the Rx part now
-        PPRZ_I2C_SEND_START(&i2c2);
+        //PPRZ_I2C_SEND_START(&i2c2);
       }
-      __enable_irq();
+      //__enable_irq();
     }
   }
 #endif
@@ -1161,23 +1246,12 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
   // if (PPRZ_I2C_IS_IDLE(p))
   if (periph->status == I2CIdle)
   {
-    if (i2c_idle(periph))
+    //if (i2c_idle(periph))
     {
 #ifdef I2C_DEBUG_LED
 	if (periph == &i2c1)
 	{
 
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-
         }
         else
 #endif
@@ -1194,12 +1268,18 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
 bool_t i2c_idle(struct i2c_periph* periph)
 {
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+#ifdef I2C_DEBUG_LED
+	if (periph == &i2c1)
+	{
+	  return TRUE;
+        }
+#endif
   if (periph->status == I2CIdle)
-  {
-    // Only read status register SR2 when it does not matter to maybe clear ADDR bits
-    return I2C_GetFlagStatus(periph->reg_addr, I2C_FLAG_BUSY) == RESET;
-  }
-  return FALSE;
+    return ! (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, regs->SR2 ) );
+  else
+    return FALSE;
 }
 
 

From 2d7fa709930182da59bbe0c24196a4ace04a6988 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Sun, 23 Oct 2011 19:30:59 +0200
Subject: [PATCH 47/74] Testing Module

---
 conf/modules/i2c_abuse_test.xml               |  15 ++
 .../modules/benchmark/i2c_abuse_test.c        | 245 ++++++++++++++++++
 .../modules/benchmark/i2c_abuse_test.h        |  52 ++++
 3 files changed, 312 insertions(+)
 create mode 100644 conf/modules/i2c_abuse_test.xml
 create mode 100644 sw/airborne/modules/benchmark/i2c_abuse_test.c
 create mode 100644 sw/airborne/modules/benchmark/i2c_abuse_test.h

diff --git a/conf/modules/i2c_abuse_test.xml b/conf/modules/i2c_abuse_test.xml
new file mode 100644
index 0000000000..c8c7039eb4
--- /dev/null
+++ b/conf/modules/i2c_abuse_test.xml
@@ -0,0 +1,15 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="benchmark">
+  <header>
+    <file name="i2c_abuse_test.h"/>
+  </header>
+  <init fun="init_i2c_abuse_test()"/>
+  <event fun="event_i2c_abuse_test()" />
+ <!-- <periodic fun="periodic_50Hz_i2c_abuse_test()" period="0.02" autorun="FALSE"/> -->
+  <makefile>
+    <!-- define name="i2c_abuse_test_MODULE_LED" value="2"/ -->
+    <file name="i2c_abuse_test.c"/>
+  </makefile>
+</module>
+
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
new file mode 100644
index 0000000000..14d1b7572f
--- /dev/null
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -0,0 +1,245 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2009  Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+#include "i2c_abuse_test.h"
+#include "led.h"
+#include "mcu_periph/i2c.h"
+
+struct i2c_transaction i2c_test1;
+struct i2c_transaction i2c_test2;
+
+uint8_t i2c_abuse_test_counter = 0;
+uint16_t i2c_abuse_test_bitrate = 1000;
+
+void init_i2c_abuse_test(void) {
+  //LED_INIT(DEMO_MODULE_LED);
+  //LED_OFF(DEMO_MODULE_LED);
+
+  i2c_test1.status = I2CTransSuccess;
+  i2c_test1.slave_addr = 0x3C;
+
+  i2c_abuse_test_counter = 0;
+  i2c_abuse_test_bitrate = 10000;
+
+  i2c_test2.status = I2CTransSuccess;
+
+}
+
+static void i2c_abuse_send_transaction(uint8_t _init)
+{
+
+  i2c_test1.slave_addr = 0x3C;
+  i2c_test1.len_w = 0;
+  i2c_test1.len_r = 0;
+
+  switch (_init)
+  {
+  case 1:
+    i2c_test1.type = I2CTransTx;
+    i2c_test1.buf[0] = 0x00;  // set to rate to 50Hz
+    i2c_test1.buf[1] = 0x00 | (0x06 << 2);
+    i2c_test1.buf[2] = 0x01<<5;
+    i2c_test1.buf[3] = 0x00;
+    i2c_test1.len_w = 4;
+    i2c_submit(&i2c2,&i2c_test1);
+    break;
+  case 2:
+    i2c_test1.type = I2CTransTx;
+    i2c_test1.buf[0] = 0x01;  // set to gain to 1 Gauss
+    i2c_test1.buf[1] = 0x01<<5;
+    i2c_test1.len_w = 2;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 3:
+    i2c_test1.type = I2CTransTx;
+    i2c_test1.buf[0] = 0x00;  // set to continuous mode
+    i2c_test1.len_w = 1;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 4:
+    i2c_test1.type = I2CTransRx;
+    i2c_test1.len_r = 1;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 5:
+    i2c_test1.type = I2CTransRx;
+    i2c_test1.len_r = 2;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 6:
+    i2c_test1.type = I2CTransRx;
+    i2c_test1.len_r = 3;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 7:
+    i2c_test1.type = I2CTransRx;
+    i2c_test1.len_r = 4;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 8:
+    i2c_test1.type = I2CTransRx;
+    i2c_test1.len_r = 5;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 9:
+    // bad addr
+    i2c_test1.slave_addr = 0x3C + 2;
+    i2c_test1.type = I2CTransTx;
+    i2c_test1.len_w = 1;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 10:
+    // 2 consecutive
+    i2c_test1.type = I2CTransTx;
+    i2c_test1.buf[0] = 0x00;  // set to continuous mode
+    i2c_test1.len_w = 1;
+    i2c_submit(&i2c2,&i2c_test1);
+  break;
+  case 11:
+    i2c_test1.slave_addr = 0x3C;
+    i2c_test1.type = I2CTransTxRx;
+    i2c_test1.len_r = 1;
+    i2c_test1.len_w = 1;
+    i2c_test1.buf[0] = 0x03;
+    i2c_submit(&i2c2, &i2c_test1);
+  break;
+  case 12:
+    i2c_test1.slave_addr = 0x3C;
+    i2c_test1.type = I2CTransTxRx;
+    i2c_test1.len_r = 2;
+    i2c_test1.len_w = 1;
+    i2c_test1.buf[0] = 0x03;
+    i2c_submit(&i2c2, &i2c_test1);
+  break;
+  case 13:
+    i2c_test1.slave_addr = 0x3C;
+    i2c_test1.type = I2CTransTxRx;
+    i2c_test1.len_r = 3;
+    i2c_test1.len_w = 1;
+    i2c_test1.buf[0] = 0x03;
+    i2c_submit(&i2c2, &i2c_test1);
+  break;
+  case 14:
+    i2c_test1.slave_addr = 0x3C;
+    i2c_test1.type = I2CTransTxRx;
+    i2c_test1.len_r = 4;
+    i2c_test1.len_w = 1;
+    i2c_test1.buf[0] = 0x03;
+    i2c_submit(&i2c2, &i2c_test1);
+  break;
+  case 15:
+    i2c_test1.slave_addr = 0x3C;
+    i2c_test1.type = I2CTransTxRx;
+    i2c_test1.len_r = 4;
+    i2c_test1.len_w = 2;
+    i2c_test1.buf[0] = 0x03;
+    i2c_submit(&i2c2, &i2c_test1);
+  break;
+  default: 
+    i2c_test1.slave_addr = 0x3C;
+    i2c_test1.type = I2CTransTxRx;
+    i2c_test1.len_r = 5;
+    i2c_test1.len_w = 1;
+    i2c_test1.buf[0] = 0x03;
+    i2c_submit(&i2c2, &i2c_test1);
+  }
+}
+
+
+void event_i2c_abuse_test(void) 
+{
+  if (i2c_idle(&i2c1))
+  {
+    LED_ON(7);	// green = idle
+    LED_OFF(6);
+  }
+  else
+  {
+    LED_ON(6); // red = busy
+    LED_OFF(7);
+  }
+
+  if (i2c_idle(&i2c2))
+  {
+    LED_ON(5);	// green = idle
+    LED_OFF(4);
+  }
+  else
+  {
+    LED_ON(4); // red = busy
+    LED_OFF(5);
+  }
+
+  // Wait for I2C transaction object to be released by the I2C driver before changing anything
+  if ((i2c_abuse_test_counter < 12) && (i2c_abuse_test_counter > 3))
+  {
+	  if ((i2c_test2.status == I2CTransFailed) || (i2c_test2.status == I2CTransSuccess))
+	  {
+		  //i2c_test2.slave_addr = 0x90;
+		  i2c_test2.type = I2CTransRx;
+		  i2c_test2.slave_addr = 0x92;
+		  i2c_test2.len_r = 2;
+	    i2c_submit(&i2c2,&i2c_test2);
+	  }
+  }
+
+  
+  if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
+  {
+	    if (i2c_abuse_test_counter < 16)
+	    {
+	       i2c_abuse_test_counter++;
+	    }
+	    else
+	    {
+		// wait until ready:
+		if (i2c_idle(&i2c2))
+		{
+			      i2c_abuse_test_counter = 1;
+
+			      i2c2_setbitrate(i2c_abuse_test_bitrate);
+
+			      i2c_abuse_test_bitrate += 17000;
+			      if (i2c_abuse_test_bitrate > 500000)
+			      {
+				i2c_abuse_test_bitrate -= 500000;
+			      }
+		  	      LED_TOGGLE(4);
+			    }
+		}
+
+	    if (i2c_abuse_test_counter < 16)
+	    {
+	      i2c_abuse_send_transaction( i2c_abuse_test_counter ); 	 
+  	      LED_TOGGLE(5);
+	    }
+  }
+}
+
+void periodic_50Hz_i2c_abuse_test(void) {
+  // LED_TOGGLE(DEMO_MODULE_LED);
+}
+
+
+
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.h b/sw/airborne/modules/benchmark/i2c_abuse_test.h
new file mode 100644
index 0000000000..e6e7b0634a
--- /dev/null
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.h
@@ -0,0 +1,52 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2009  C. De Wagter
+ *
+ * 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 I2C_ABUSE_TEST_module.h
+ *
+ * Total I2C Abuse:
+ *
+ * -all transaction types: T1 T2 T3 T4 R1 R2 R3 R4 T1R1 T2R1 T1R2 T1R3 T1R4 T1R5 T2R5
+ * -all bitrates: 1k (way too slow) to 1M (way to fast)
+ * -occasional Short circuit (simulate bus capacitance or EMI errors)
+ * -variable bus load: from empty to full stack
+ *
+ * -Connect LED to MosFet that pulls-down the SCL and SDA lines
+ */
+
+#ifndef I2C_ABUSE_TEST_MODULE_H
+#define I2C_ABUSE_TEST_MODULE_H
+
+#ifndef I2C_ABUSE_SHORT_SCL_LED
+#define I2C_ABUSE_SHORT_SCL_LED 2
+#endif
+
+#ifndef I2C_ABUSE_SHORT_SDA_LED
+#define I2C_ABUSE_SHORT_SDA_LED 3
+#endif
+
+void init_i2c_abuse_test(void);
+void event_i2c_abuse_test(void);
+void periodic_50Hz_i2c_abuse_test(void);
+
+#endif

From 8d6ed4da3c861f601c311fd78e36ebf650f7da30 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 24 Oct 2011 09:11:15 +0200
Subject: [PATCH 48/74] We have identified that the event and error interrupts
 can be called 'during' each other causing disaster. When disabling the irq
 the 2nd interrupt is still triggered even if the irq condition was removed by
 the first, leading to other types of issues...

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 134 ++++++++++++++----
 .../modules/benchmark/i2c_abuse_test.c        |   2 +-
 2 files changed, 104 insertions(+), 32 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 09c7a02dc3..25fa421fff 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -136,6 +136,12 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 
 #define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
 
+// Critical Zones
+
+#define __I2C_REG_CRITICAL_ZONE_START	
+#define __I2C_REG_CRITICAL_ZONE_STOP
+
+
 //////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////
 
@@ -227,6 +233,10 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
   LED2_OFF();    
   LED1_OFF();
 
+//#define I2C_DEBUG_LED_CONTROL
+#ifdef I2C_DEBUG_LED_CONTROL
+
+
   LED1_ON();
 
   // 1 Anything CR?
@@ -251,7 +261,8 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
   LED2_OFF();    
     
   LED1_OFF();
-  
+#endif  
+
 }
 #endif
 
@@ -322,7 +333,8 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 enum STMI2CSubTransactionStatus {
   STMI2C_SubTra_Busy,
   STMI2C_SubTra_Ready_StopRequested,
-  STMI2C_SubTra_Ready
+  STMI2C_SubTra_Ready,
+  STMI2C_SubTra_Error
 };
 
 static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
@@ -389,6 +401,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, str
 
       return STMI2C_SubTra_Ready;
   }
+  else // Hardware error
+  {
+    return STMI2C_SubTra_Error;
+  }
 
   return STMI2C_SubTra_Busy;
 }
@@ -411,7 +427,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
     // --- next to steps MUST be executed together to avoid missing the stop
-    __disable_irq();
+    __I2C_REG_CRITICAL_ZONE_START;
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
@@ -419,7 +435,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     // Schedule a Stop
     PPRZ_I2C_SEND_STOP(regs);
 
-    __enable_irq();
+    __I2C_REG_CRITICAL_ZONE_STOP;
     // --- end of critical zone -----------
 
     // Enable the RXNE to get the result
@@ -435,6 +451,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
 
     return STMI2C_SubTra_Ready_StopRequested;
   }
+  else // Hardware error
+  {
+    return STMI2C_SubTra_Error;
+  }
 
   return STMI2C_SubTra_Busy;
 }
@@ -459,7 +479,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
       
     // --- make absolutely sure this command is not delayed too much after the previous: 
-    __disable_irq();
+    __I2C_REG_CRITICAL_ZONE_START;
     //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
@@ -467,7 +487,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // Disable the RXNE and wait for BTF
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
 
-    __enable_irq();
+    __I2C_REG_CRITICAL_ZONE_STOP;
     // --- end of critical zone -----------
   }
   // Receive buffer if full, master is halted: BTF
@@ -485,6 +505,10 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
 
     return STMI2C_SubTra_Ready_StopRequested;
   }
+  else // Hardware error
+  {
+    return STMI2C_SubTra_Error;
+  }
 
   return STMI2C_SubTra_Busy;
 }
@@ -565,7 +589,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // And I2C is halted so we have time
 
     // --- Make absolutely sure the next 2 I2C actions are performed with no delay
-    __disable_irq();
+    __I2C_REG_CRITICAL_ZONE_START;
 
     // First we clear the ACK while the SCL is held low by BTF
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
@@ -577,7 +601,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
     PPRZ_I2C_SEND_STOP(regs);
 
-    __enable_irq();
+    __I2C_REG_CRITICAL_ZONE_STOP;
     // --- end of critical zone -----------
 
     // read the byte2 we had in the buffer (BTF means 2 bytes available)
@@ -588,15 +612,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
-  else
+  else // Hardware error
   {
-    // Error
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED2_OFF();
-	LED1_OFF();
-#endif
+    return STMI2C_SubTra_Error;
   }
 
   return STMI2C_SubTra_Busy;
@@ -609,6 +627,8 @@ static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
 
   regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
 
+  //regs->CR1 &= ~ I2C_CR1_BIT_PE;		// Disable Periferial
+  //regs->CR1 |=   I2C_CR1_BIT_PE;		// Enable Periferial
 
   // Start Condition Was Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
@@ -620,7 +640,7 @@ static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
   if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // ADDR: Cleared by software when reading SR1 and then SR2
-    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+    uint16_t SR2 __attribute__ ((unused)) = SR2;
   }
   // Byte Transfer Finished
   if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
@@ -629,6 +649,15 @@ static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
     uint8_t dummy __attribute__ ((unused)) = regs->DR;
     regs->DR = 0x00;
   }
+
+
+  // Still have a start sheduled
+//  if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_START, regs->CR1) )
+  {
+    // Clear pending start conditions
+//    regs->CR1 &= ~ I2C_CR1_BIT_START;
+  }
+
 }
 
 
@@ -734,12 +763,6 @@ static inline void i2c_irq(struct i2c_periph *periph)
   // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
   
-#ifdef I2C_DEBUG_LED
-  LED1_ON();
-  LED1_OFF();
-#endif
-
-
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   //
@@ -765,12 +788,12 @@ static inline void i2c_irq(struct i2c_periph *periph)
     // (which can happen if an event was sheduled just before a bus error occurs)
     // then its easy: just stop: clear all interrupt generating bits  
 
-    // Clear Running Events
-    stmi2c_clear_pending_interrupts(regs);
-
     // Count The Errors
     i2c_error(periph);
 
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
     // Mark this as a special error
     periph->errors->unexpected_event_cnt++;
 
@@ -806,11 +829,26 @@ static inline void i2c_irq(struct i2c_periph *periph)
     // Make sure a TxRx does not Restart
     trans->type = I2CTransRx;
 
-    // Clear Running Events
-    stmi2c_clear_pending_interrupts(regs);
+/*
+    // There are 2 types of errors: some need a STOP, some better do without: Following will not get an extra stop
+    if (
+           // Lost Arbitration
+           (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_ARLO, regs->SR1 ) )
+           // Buss Error When Master Only
+        || ((BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_BUS,  regs->SR1 ) )  &&  (!BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL,  regs->SR2 ) ))
+        || (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_OVR,  regs->SR1 ) )
+       )
+    {
+      ret = STMI2C_SubTra_Error;
+    }
+*/
 
     // Count The Errors
     i2c_error(periph);
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
   }
 
 
@@ -854,10 +892,32 @@ static inline void i2c_irq(struct i2c_periph *periph)
         // Program a stop
         PPRZ_I2C_SEND_STOP(regs);
 
-        // Silent any BTF that would occur before STOP is executed
-        regs->DR = 0x00;
+        // Silent further BTF
+        regs->DR = 0;
       }
 
+      // In case of unexpected condition: e.g. not slave, no event
+      if (ret == STMI2C_SubTra_Error)
+      {
+
+        trans->status = I2CTransFailed;
+
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+#endif
+
+        LED_SHOW_ACTIVE_BITS(regs);
+
+        // Clear Running Events
+        stmi2c_clear_pending_interrupts(regs);
+
+      }
+
+
       // Jump to the next transaction
       periph->trans_extract_idx++;
       if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
@@ -1132,11 +1192,23 @@ void i2c2_setbitrate(int bitrate)
 
 
 void i2c2_ev_irq_handler(void) {
+        __disable_irq();
+#ifdef I2C_DEBUG_LED
+  LED1_ON();
+  LED1_OFF();
+#endif
   i2c_irq(&i2c2);
+        __enable_irq();
 }
 
 void i2c2_er_irq_handler(void) {
+        __disable_irq();
+#ifdef I2C_DEBUG_LED
+  LED2_ON();
+  LED2_OFF();
+#endif
   i2c_irq(&i2c2);
+        __enable_irq();
 }
 
 #endif /* USE_I2C2 */
@@ -1145,7 +1217,7 @@ void i2c2_er_irq_handler(void) {
 void i2c_event(void) 
 {
   static uint32_t cnt = 0;
-  I2C_TypeDef *regs;
+  //I2C_TypeDef *regs;
   cnt++;
   if (cnt > 10000) cnt = 0;
 
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 14d1b7572f..046eb740d3 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *
- * Copyright (C) 2009  Gautier Hattenberger
+ * Copyright (C) 2009  C. De Wagter
  *
  * This file is part of paparazzi.
  *

From 13d23b8bbcc19a7e033480d50812c08c99e1fbe0 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 24 Oct 2011 09:30:27 +0200
Subject: [PATCH 49/74] Test airframe

---
 conf/airframes/CDW/debug_i2c.xml | 279 +++++++++++++++++++++++++++++++
 1 file changed, 279 insertions(+)
 create mode 100644 conf/airframes/CDW/debug_i2c.xml

diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
new file mode 100644
index 0000000000..7dd9a616eb
--- /dev/null
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -0,0 +1,279 @@
+<!-- this is an asctec frame equiped with  Lisa/L#3 and asctec V2 controllers -->
+
+<airframe name="booz2_a7">
+
+  <servos min="0" neutral="0" max="0xff">
+    <servo name="FRONT"   no="0" min="0" neutral="0" max="255"/>
+    <servo name="BACK"    no="1" min="0" neutral="0" max="255"/>
+    <servo name="LEFT"    no="2" min="0" neutral="0" max="255"/>
+    <servo name="RIGHT"   no="3" min="0" neutral="0" max="255"/>
+  </servos>
+
+  <commands>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <!-- for the sim -->
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+  </section>
+
+
+  <section name="SUPERVISION" prefix="SUPERVISION_">
+    <define name="MIN_MOTOR" value="3"/>
+    <define name="MAX_MOTOR" value="200"/>
+    <define name="TRIM_A" value="0"/>
+    <define name="TRIM_E" value="0"/>
+    <define name="TRIM_R" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
+    <define name="ROLL_COEF"   value="{  0  ,    0,  256, -256 }"/>
+    <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
+    <define name="YAW_COEF"    value="{  256,  256, -256, -256 }"/>
+    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+
+    <define name="GYRO_P_NEUTRAL" value="32858"/>
+    <define name="GYRO_Q_NEUTRAL" value="33152"/>
+    <define name="GYRO_R_NEUTRAL" value="31779"/>
+    <define name="GYRO_P_SENS" value=" 1.101357422" integer="16"/>
+    <define name="GYRO_Q_SENS" value=" 1.122670898" integer="16"/>
+    <define name="GYRO_R_SENS" value=" 1.104890137" integer="16"/>
+
+
+    <define name="ACCEL_X_NEUTRAL" value="32899"/>
+    <define name="ACCEL_Y_NEUTRAL" value="33281"/>
+    <define name="ACCEL_Z_NEUTRAL" value="32569"/>
+    <define name="ACCEL_X_SENS" value="2.57702956051" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="2.57835230627" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="2.54311399868" integer="16"/>
+
+    <define name="MAG_X_NEUTRAL" value="58"/>
+    <define name="MAG_Y_NEUTRAL" value="92"/>
+    <define name="MAG_Z_NEUTRAL" value="-3"/>
+    <define name="MAG_X_SENS" value="4.70018395734" integer="16"/>
+    <define name="MAG_Y_SENS" value="4.884119848" integer="16"/>
+    <define name="MAG_Z_SENS" value="2.59926404993" integer="16"/>
+
+    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 135.)"/>
+
+  </section>
+
+ <section name="AUTOPILOT">
+   <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+   <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+   <define name="MODE_AUTO2"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+ </section>
+
+ <section name="BAT">
+   <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+   <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+   <define name="BATTERY_SENS"   value="0.48" integer="16"/>
+ </section>
+
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+
+    <define name="GAIN_P" value="-400"/>
+    <define name="GAIN_Q" value="-400"/>
+    <define name="GAIN_R" value="-350"/>
+
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="DEADBAND_R"     value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
+
+    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+   <!--  gaui props
+    <define name="PHI_PGAIN"  value="-900"/>
+    <define name="PHI_DGAIN"  value="-200"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-900"/>
+    <define name="THETA_DGAIN"  value="-200"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-900"/>
+    <define name="PSI_DGAIN"  value="-200"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <define name="PHI_DDGAIN"   value=" 200"/>
+    <define name="THETA_DDGAIN" value=" 200"/>
+    <define name="PSI_DDGAIN"   value=" 200"/>
+  -->
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-2000"/>
+    <define name="PHI_DGAIN"  value="-400"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-2000"/>
+    <define name="THETA_DGAIN"  value="-400"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-2000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
+
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="H_X" value=" 0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value=" 0.85490967783446"/>
+  </section>
+
+
+  <section name="INS" prefix="INS_">
+    <define name="BARO_SENS" value="10." integer="16"/>
+    <define name="UNTILT_ACCEL" value="1"/>
+  </section>
+
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+   <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+   <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+   <define name="MAX_SUM_ERR" value="2000000"/>
+
+<!--
+   <define name="HOVER_KP"    value="-500"/>
+   <define name="HOVER_KD"    value="-200"/>
+   <define name="HOVER_KI"    value="0"/>
+-->
+   <define name="HOVER_KP"     value="-150"/>
+   <define name="HOVER_KD"     value="-80"/>
+   <define name="HOVER_KI"     value="0"/>
+
+   <!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
+   <define name="RC_CLIMB_COEF" value ="163"/>
+   <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
+   <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
+    <!-- <define name="INV_M" value ="0.2"/> -->
+  </section>
+
+
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+   <define name="PGAIN" value="-50"/>
+   <define name="DGAIN" value="-100"/>
+   <define name="IGAIN" value="-15"/>
+   <define name="NGAIN" value="-0"/>
+ </section>
+
+ <section name="MISC">
+   <define name="FACE_REINJ_1"  value="1024"/>
+ </section>
+
+ <section name="SIMULATOR" prefix="NPS_">
+   <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
+   <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
+ </section>
+
+
+ <modules main_freq="512">
+   <load name="i2c_abuse_test.xml"/>
+ </modules>
+
+  <firmware name="rotorcraft">
+<!--     <define name="USE_PERSISTENT_SETTINGS"/> -->
+<!--     <define name="USE_INS_NAV_INIT"/> -->
+     <define name="USE_ADAPT_HOVER"/>
+     <define name="FAILSAFE_GROUND_DETECT"/>
+     <define name="USE_GPS_ACC4R"/>
+     <target name="ap" board="lisa_l_1.0">
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="actuators"     type="asctec"/>
+    </target>
+    <target name="sim" board="pc">
+      <subsystem name="fdm"           type="nps"/>
+      <subsystem name="radio_control" type="ppm"/>
+      <subsystem name="actuators"     type="mkk"/>
+    </target>
+    <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="imu"           type="b2_v1.1"/>
+    <subsystem name="gps"           type="ublox">
+      <configure name="GPS_BAUD"    value="B57600"/>
+    </subsystem>
+    <!--subsystem name="ahrs"          type="int_cmpl_euler"/-->
+    <subsystem name="stabilization"   type="euler"/>
+    <subsystem name="ahrs"          type="int_cmpl_quat"/>
+    <subsystem name="ins"           type="hff"/>
+  </firmware>
+
+  <firmware name="lisa_l_test_progs">
+    <target name="test_led"                 board="lisa_l_1.0"/>
+    <target name="test_uart"                board="lisa_l_1.0"/>
+    <target name="test_servos"              board="lisa_l_1.0"/>
+    <target name="test_telemetry"           board="lisa_l_1.0"/>
+    <target name="test_baro"                board="lisa_l_1.0"/>
+    <target name="test_imu_b2"              board="lisa_l_1.0"/>
+    <target name="test_imu_b2_2"            board="lisa_l_1.0"/>
+    <target name="test_imu_aspirin"         board="lisa_l_1.0"/>
+    <target name="test_rc_spektrum"         board="lisa_l_1.0"/>
+    <target name="test_rc_ppm"              board="lisa_l_1.0"/>
+    <target name="test_adc"                 board="lisa_l_1.0"/>
+    <target name="test_hmc5843"             board="lisa_l_1.0"/>
+    <target name="test_itg3200"             board="lisa_l_1.0"/>
+    <target name="test_adxl345"             board="lisa_l_1.0"/>
+    <target name="test_esc_mkk_simple"      board="lisa_l_1.0"/>
+    <target name="test_esc_asctecv1_simple" board="lisa_l_1.0"/>
+    <target name="test_actuators_mkk"       board="lisa_l_1.0"/>
+    <target name="test_actuators_asctecv1"  board="lisa_l_1.0"/>
+    <target name="tunnel_sw"                board="lisa_l_1.0"/>
+    <target name="tunnel_hw"                board="lisa_l_1.0"/>
+  </firmware>
+
+  <firmware name="lisa_passthrough">
+    <target name="overo_test_passthrough" board="lisa_l_1.0"  >
+      <param name="HOST"          value="A7"/>
+      <param name="USER"          value=""/>
+      <param name="TARGET_DIR"    value="~"/>
+      <param name="PERIODIC_FREQ" value="512"/>
+    </target>
+    <target name="stm_passthrough" board="lisa_l_1.0">
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="imu" type="b2_v1.1"/>
+    </target>
+ </firmware>
+
+
+</airframe>

From 76ca557f4dba36b4b122c26ce8b88cc95a138dd3 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 24 Oct 2011 09:35:18 +0200
Subject: [PATCH 50/74] Revert magneto driver to default

---
 sw/airborne/peripherals/hmc5843.c | 262 +++++++++++-------------------
 1 file changed, 94 insertions(+), 168 deletions(-)

diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index ab4e105ae6..4eac86d75a 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -1,5 +1,5 @@
 #include "peripherals/hmc5843.h"
-
+#include "mcu_periph/i2c.h"
 #include "led.h"
 
 #define HMC5843_TIMEOUT 100
@@ -9,196 +9,122 @@
 struct Hmc5843 hmc5843;
 void exti9_5_irq_handler(void);
 
-struct i2c_transaction i2c_test2;
-
+#ifndef HMC5843_I2C_DEVICE
+#define HMC5843_I2C_DEVICE i2c2
+#endif
 
 void hmc5843_init(void)
 {
   hmc5843.i2c_trans.status = I2CTransSuccess;
   hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
 
+#ifndef HMC5843_NO_IRQ
   hmc5843_arch_init();
-  hmc5843.initialized = 0;
-  hmc5843.timeout = 10000;
+#endif
 }
 
-static void hmc_send_config(uint8_t _init)
+// blocking, only intended to be called for initialization
+static void send_config(void)
 {
-  //i2c_test2.slave_addr = 0x90;
-  i2c_test2.type = I2CTransTx;
-  i2c_test2.slave_addr = 0x00;
-  i2c_test2.len_w = 1;
-  i2c_test2.buf[0] = 0x06;
+  hmc5843.i2c_trans.type = I2CTransTx;
+  hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
+  hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
+  hmc5843.i2c_trans.len_w = 2;
+  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
+
+  hmc5843.i2c_trans.type = I2CTransTx;
+  hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
+  hmc5843.i2c_trans.buf[1] = 0x01<<5;
+  hmc5843.i2c_trans.len_w = 2;
+  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
+
+  hmc5843.i2c_trans.type = I2CTransTx;
+  hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
+  hmc5843.i2c_trans.buf[1] = 0x00;
+  hmc5843.i2c_trans.len_w = 2;
+  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
-  hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-  hmc5843.i2c_trans.len_w = 0;
-  hmc5843.i2c_trans.len_r = 0;
-  switch (_init)
-  {
-  case 1:
-    hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
-    hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
-    hmc5843.i2c_trans.buf[2] = 0x01<<5;
-    hmc5843.i2c_trans.buf[3] = 0x00;
-    hmc5843.i2c_trans.len_w = 4;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    break;
-  case 2:
-    hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
-    hmc5843.i2c_trans.buf[1] = 0x01<<5;
-    hmc5843.i2c_trans.len_w = 2;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 3:
-    hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to continuous mode
-    hmc5843.i2c_trans.len_w = 1;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 4:
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 1;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 5:
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 2;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 6:
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 3;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 7:
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 4;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 8:
-    hmc5843.i2c_trans.type = I2CTransRx;
-    hmc5843.i2c_trans.len_r = 5;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 9:
-    // bad addr
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR + 2;
-    hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.len_w = 1;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-  break;
-  case 10:
-    // 2 consecutive
-    hmc5843.i2c_trans.type = I2CTransTx;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to continuous mode
-    hmc5843.i2c_trans.len_w = 1;
-    i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    //i2c_submit(&i2c2,&i2c_test2);
-  break;
-  case 11:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 1;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
-  break;
-  case 12:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 2;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
-  break;
-  case 13:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 3;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
-  break;
-  case 14:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 4;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
-  break;
-  case 15:
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 4;
-    hmc5843.i2c_trans.len_w = 2;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
-  break;
-  default: 
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
-    hmc5843.i2c_trans.type = I2CTransTxRx;
-    hmc5843.i2c_trans.len_r = 5;
-    hmc5843.i2c_trans.len_w = 1;
-    hmc5843.i2c_trans.buf[0] = HMC5843_REG_DATXM;
-    i2c_submit(&i2c2, &hmc5843.i2c_trans);
-  }
 }
 
+#ifdef HMC5843_NO_IRQ
+volatile uint8_t fake_mag_eoc = 0;
+static uint8_t mag_eoc(void)
+{
+  return fake_mag_eoc;
+}
+#endif
+
 void hmc5843_idle_task(void)
 {
-  if (i2c_idle(&i2c2))
-  {
-    LED_ON(7);	// green = idle
-    LED_OFF(6);
-  }
-  else
-  {
-    LED_ON(6); // red = busy
-    LED_OFF(7);
+  if (hmc5843.i2c_trans.status == I2CTransFailed) {
+    hmc5843.sent_tx = 0;
+    hmc5843.sent_rx = 0;
   }
 
-  // Wait for I2C transaction object to be released by the I2C driver before changing anything
-  if (i2c_idle(&i2c2))
-  {
-	  if ((hmc5843.i2c_trans.status == I2CTransFailed) || (hmc5843.i2c_trans.status == I2CTransSuccess))
-	  {
-	    LED_ON(5);
-	    LED_OFF(4);
-	    if (hmc5843.initialized < 16)
-	    {
-	       hmc5843.initialized++;
-	    }
-	    else
-	    {
-	      hmc5843.initialized = 1;
+  if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return;
 
-	      i2c2_setbitrate(hmc5843.timeout);
+  if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) {
+    if (HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)) {
+      hmc5843.i2c_trans.type = I2CTransTx;
+      hmc5843.i2c_trans.len_w = 1;
+      hmc5843.i2c_trans.buf[0] = 0x3;
+      i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+      hmc5843.sent_tx = 1;
+      return;
+    }
+  }
 
-	      hmc5843.timeout += 10000;
-              if (hmc5843.timeout > 800000)
-              {
-                hmc5843.timeout = 10000;
-              }
-	    }
+  if (hmc5843.sent_tx) {
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 6;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = 0x3;
+    i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+    hmc5843.sent_rx = 1;
+    hmc5843.sent_tx = 0;
+    return;
+  }
 
-	    //if (hmc5843.initialized < 4)
-	    {
-	      hmc_send_config( hmc5843.initialized ); 	 
-	    }
-
-	  }
-	  else
-	  {
-	    LED_ON(4);
-	    LED_OFF(5);
-	  }
+  if (hmc5843.sent_rx && hmc5843.i2c_trans.status == I2CTransSuccess) {
+    hmc5843.sent_rx = 0;
+    hmc5843.sent_tx = 0;
+    hmc5843.timeout = 0;
+    hmc5843.data_available = TRUE;
+    memcpy(hmc5843.data.buf, (const void*) hmc5843.i2c_trans.buf, 6);
+    for (int i = 0; i < 3; i++) {
+      hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
+    }
   }
 }
 
 void hmc5843_periodic(void)
 {
-  //hmc5843.timeout++;
+  if (!hmc5843.initialized) {
+    send_config();
+    hmc5843.initialized = TRUE;
+  } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)){
+#ifdef USE_HMC59843_ARCH_RESET
+    hmc5843_arch_reset();
+#endif
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = 0x3;
+    i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
+
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 6;
+    i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
+    hmc5843.timeout = 0;
+  }
+
+#ifdef HMC5843_NO_IRQ
+  // < 50Hz
+  fake_mag_eoc = 1;
+#endif
+
 }

From 7845ee1daeb8dfa6d17c88ff7d0aa338bf2dffd8 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 24 Oct 2011 09:56:06 +0200
Subject: [PATCH 51/74] Change Bitrate Interface Upgrade

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 39 ++++++++++---------
 sw/airborne/mcu_periph/i2c.h                  |  2 +-
 .../modules/benchmark/i2c_abuse_test.c        |  2 +-
 3 files changed, 22 insertions(+), 21 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 09c7a02dc3..4375369c88 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -5,8 +5,6 @@
 #include <stm32/flash.h>
 #include <stm32/misc.h>
 
-//#include "led.h" 
-
 #define I2C_DEBUG_LED
 
 /////////// DEBUGGING //////////////
@@ -14,6 +12,7 @@
 
 
 #ifdef I2C_DEBUG_LED
+
 static inline void LED1_ON(void)
 {
   GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
@@ -1103,13 +1102,27 @@ void i2c2_hw_init(void) {
 }
 
 
-void i2c2_setbitrate(int bitrate)
+
+void i2c2_ev_irq_handler(void) {
+  i2c_irq(&i2c2);
+}
+
+void i2c2_er_irq_handler(void) {
+  i2c_irq(&i2c2);
+}
+
+#endif /* USE_I2C2 */
+
+
+void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
 {
-  if (i2c_idle(&i2c2))
+  if (i2c_idle(periph))
   {
-    I2C2_InitStruct.I2C_ClockSpeed = bitrate;
-    I2C_Init(I2C2, i2c2.init_struct);
-  
+    if (periph == &i2c2)
+    {
+      I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+      I2C_Init(I2C2, i2c2.init_struct);
+    }
 
 #ifdef I2C_DEBUG_LED
         __disable_irq();
@@ -1130,18 +1143,6 @@ void i2c2_setbitrate(int bitrate)
 }
 
 
-
-void i2c2_ev_irq_handler(void) {
-  i2c_irq(&i2c2);
-}
-
-void i2c2_er_irq_handler(void) {
-  i2c_irq(&i2c2);
-}
-
-#endif /* USE_I2C2 */
-
-
 void i2c_event(void) 
 {
   static uint32_t cnt = 0;
diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h
index 8d9d3269cb..b0df164c10 100644
--- a/sw/airborne/mcu_periph/i2c.h
+++ b/sw/airborne/mcu_periph/i2c.h
@@ -133,7 +133,7 @@ extern void i2c2_init(void);
 extern void   i2c_init(struct i2c_periph* p);
 extern bool_t i2c_idle(struct i2c_periph* p);
 extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t);
-extern void   i2c2_setbitrate(int bitrate);
+extern void   i2c_setbitrate(struct i2c_periph* p, int bitrate);
 extern void   i2c_event(void);
 
 
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 14d1b7572f..11b947b375 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -218,7 +218,7 @@ void event_i2c_abuse_test(void)
 		{
 			      i2c_abuse_test_counter = 1;
 
-			      i2c2_setbitrate(i2c_abuse_test_bitrate);
+			      i2c_setbitrate(&i2c2, i2c_abuse_test_bitrate);
 
 			      i2c_abuse_test_bitrate += 17000;
 			      if (i2c_abuse_test_bitrate > 500000)

From e5b15fede43bce8ee1cc4f64dddfeb96e71a7c7a Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 24 Oct 2011 16:58:51 +0200
Subject: [PATCH 52/74] IVY2serial

---
 .gitignore                          |   1 +
 sw/ground_segment/misc/Makefile     |   6 +-
 sw/ground_segment/misc/ivy2serial.c | 217 ++++++++++++++++++++++++++++
 3 files changed, 223 insertions(+), 1 deletion(-)
 create mode 100644 sw/ground_segment/misc/ivy2serial.c

diff --git a/.gitignore b/.gitignore
index 4e3b07d886..b37cdc68b5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -80,6 +80,7 @@
 /sw/ground_segment/tmtc/ivy2udp
 /sw/ground_segment/tmtc/server
 /sw/ground_segment/tmtc/diadec
+/sw/ground_segment/tmtc/misc/ivy2serial
 
 # /sw/ground_segment/joystick
 /sw/ground_segment/joystick/input2ivy
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
index 35ff00d8c9..d2c1539164 100644
--- a/sw/ground_segment/misc/Makefile
+++ b/sw/ground_segment/misc/Makefile
@@ -7,7 +7,7 @@ else
 endif
 
 
-all: davis2ivy kestrel2ivy
+all: davis2ivy kestrel2ivy ivy2serial
 
 clean:
 	rm *.o davis2ivy kestrel2ivy
@@ -18,5 +18,9 @@ davis2ivy: davis2ivy.o
 kestrel2ivy: kestrel2ivy.o
 	g++ -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy
 
+ivy2serial: ivy2serial.o
+	g++ -o ivy2serial ivy2serial.o $(LIBRARYS) -livy	
+
+
 %.o : %.c
 	gcc -c -O2 -Wall -I /opt/local/include/  $<
diff --git a/sw/ground_segment/misc/ivy2serial.c b/sw/ground_segment/misc/ivy2serial.c
new file mode 100644
index 0000000000..e006499f82
--- /dev/null
+++ b/sw/ground_segment/misc/ivy2serial.c
@@ -0,0 +1,217 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+
+#include <signal.h>
+
+#include <Ivy/ivy.h>
+#include <Ivy/ivyloop.h>
+#include <Ivy/timer.h>
+#include <Ivy/version.h>
+
+//////////////////////////////////////////////////////////////////////////////////
+// SETTINGS
+//////////////////////////////////////////////////////////////////////////////////
+
+// Serial Repeat Rate
+long delay = 1000;
+
+// local_uav Number
+unsigned char send_ac_id = 5;
+
+//////////////////////////////////////////////////////////////////////////////////
+// local_uav DATA
+//////////////////////////////////////////////////////////////////////////////////
+
+
+struct _uav_type_
+{
+  // Header
+  unsigned char header;
+
+  // Data
+  unsigned char ac_id;
+  short int phi, theta, psi;
+  int utm_east,utm_north,utm_z;
+  unsigned char utm_zone;
+  
+  // Footer
+  unsigned char footer;
+} 
+__attribute__((packed))
+
+local_uav, remote_uav;
+
+//////////////////////////////////////////////////////////////////////////////////
+// IVY Reader
+//////////////////////////////////////////////////////////////////////////////////
+
+
+static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+  unsigned char id = atoi(argv[0]);
+
+  if (id != send_ac_id)
+  {
+    printf("NEGLECT: %d\n",id);
+    return;
+  }
+
+/*
+   <message name="ATTITUDE" id="6">
+     <field name="phi"   type="float" unit="rad" alt_unit="deg"/>
+     <field name="psi"   type="float" unit="rad" alt_unit="deg"/>
+     <field name="theta" type="float" unit="rad" alt_unit="deg"/>
+   </message>
+*/
+
+  local_uav.ac_id = id;
+  local_uav.phi   = (short int) (atof(argv[1]) * 1000.0);
+  local_uav.theta = (short int) (atof(argv[3]) * 1000.0);
+  local_uav.psi   = (short int) (atof(argv[2]) * 1000.0);
+
+}
+
+static void on_Gps(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+  unsigned char id = atoi(argv[0]);
+
+  if (id != send_ac_id)
+  {
+    printf("NEGLECT: %d\n",id);
+    return;
+  }
+
+/*
+   <message name="GPS" id="8">
+     <field name="mode"       type="uint8"  unit="byte_mask"/>
+     <field name="utm_east"   type="int32"  unit="cm" alt_unit="m"/>
+     <field name="utm_north"  type="int32"  unit="cm" alt_unit="m"/>
+     <field name="course"     type="int16"  unit="decideg" alt_unit="deg"/>
+     <field name="alt"        type="int32"  unit="mm" alt_unit="m"/>
+     <field name="speed"      type="uint16" unit="cm/s" alt_unit="m/s"/>
+     <field name="climb"      type="int16"  unit="cm/s" alt_unit="m/s"/>
+     <field name="week"       type="uint16" unit="weeks"/>
+     <field name="itow"       type="uint32" unit="ms"/>
+     <field name="utm_zone"   type="uint8"/>
+     <field name="gps_nb_err" type="uint8"/>
+   </message>
+*/
+
+  local_uav.ac_id = id;
+  local_uav.utm_east = atoi(argv[2]);
+  local_uav.utm_north = atoi(argv[3]);
+  local_uav.utm_z = atoi(argv[5]);
+  local_uav.utm_zone = atoi(argv[10]);
+
+  printf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi);
+  printf("GPS ac=%d %d %d %d %d\n",local_uav.ac_id, local_uav.utm_east, local_uav.utm_north, local_uav.utm_z, local_uav.utm_zone);
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// SERIAL PORT
+//////////////////////////////////////////////////////////////////////////////////
+
+// pointer
+int fd;
+
+/// Open
+void open_port(const char* device) {
+  fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fd == -1) {
+    fprintf(stderr, "open_port: unable to open device %s - ", device);
+    perror(NULL);
+    exit(EXIT_FAILURE);
+  }
+  // setup connection options
+  struct termios options;
+
+  // get the current options
+  tcgetattr(fd, &options);
+
+  // set local mode, enable receiver, set comm. options:
+  // 8 data bits, 1 stop bit, no parity, 9600 Baud
+  options.c_cflag = CLOCAL | CREAD | CS8 | B9600;
+
+  // write options back to port
+  tcsetattr(fd, TCSANOW, &options);
+}
+
+void send_port(void)
+{
+  int bytes = write(fd, &local_uav, sizeof(local_uav));
+  printf("SENT: %d bytes\n",bytes);
+}
+
+void read_port(void)
+{
+}
+
+void close_port(void)
+{
+  close(fd);
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// TIMER
+//////////////////////////////////////////////////////////////////////////////////
+
+// Timer
+void handle_timer (TimerId id, void *data, unsigned long delta) {
+  printf("TIMER\n");
+  send_port();
+}
+
+TimerId tid;
+
+/// Handler for Ctrl-C, exits the main loop
+void sigint_handler(int sig) {
+  printf("\nCLEAN STOP\n");
+  IvyStop();
+  TimerRemove(tid);
+  close_port(); 
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// MAIN
+//////////////////////////////////////////////////////////////////////////////////
+
+int main ( int argc, char** argv) 
+{
+  int s = sizeof(local_uav);
+ 
+  if (argc < 3)
+  {
+    printf("Use: ivy2serial ac_id serial_device\n");
+    return -1;
+  }
+  
+  send_ac_id = atoi(argv[1]);
+
+  printf("Listening to AC=%d, \nSending Size of Data = %d \n",send_ac_id, s);
+
+  // make Ctrl-C stop the main loop and clean up properly
+  signal(SIGINT, sigint_handler);
+
+  open_port(argv[2]);
+
+  // create timer (Ivy)
+  tid = TimerRepeatAfter (0, delay, handle_timer, 0);
+
+
+  IvyInit ("IVY <-> Serial", "IVY <-> Serial READY", NULL, NULL, NULL, NULL);
+  IvyBindMsg(on_Attitude, NULL, "^(\\S*) ATTITUDE (\\S*) (\\S*) (\\S*)");
+  IvyBindMsg(on_Gps, NULL, "^(\\S*) GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+  IvyStart("127.255.255.255");
+  
+  IvyMainLoop ();
+
+  return 0;
+}
+

From f6a9a50fcd24f3e0512288760a3c3a961d9e57da Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 22 Nov 2011 09:59:36 +0100
Subject: [PATCH 53/74] Fix trailing spaces

---
 .../TU_Delft/EasyStartPanTiltCHIMU_SPI.xml    |  2 +-
 conf/airframes/TU_Delft/skywalkerfiber.xml    | 14 +--
 .../subsystems/fixedwing/autopilot.makefile   |  2 +-
 conf/modules/gps_ubx_ucenter.xml              |  2 +-
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 96 +++++++++----------
 sw/airborne/arch/stm32/sys_time_hw.h          |  2 +-
 .../modules/benchmark/i2c_abuse_test.c        |  8 +-
 sw/airborne/modules/gps/gps_ubx_ucenter.c     |  8 +-
 sw/airborne/modules/gps/gps_ubx_ucenter.h     |  2 +-
 sw/airborne/modules/ins/ins_chimu_spi.c       |  4 +-
 10 files changed, 70 insertions(+), 70 deletions(-)

diff --git a/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml b/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml
index 9286e53a56..96d2fce242 100644
--- a/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml
+++ b/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<!-- 
+<!--
 	YAPA + XSens + XBee
 -->
 
diff --git a/conf/airframes/TU_Delft/skywalkerfiber.xml b/conf/airframes/TU_Delft/skywalkerfiber.xml
index 000a3c7c25..a962c1f226 100644
--- a/conf/airframes/TU_Delft/skywalkerfiber.xml
+++ b/conf/airframes/TU_Delft/skywalkerfiber.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<!-- 
+<!--
      Skywalker Fiberglass "GraySky" FPV 168 (http://www..com/)
      YAPA v2  (http://paparazzi.enac.fr/wiki/Yapa#Yapa_v2)
      With modified power routing
@@ -51,7 +51,7 @@
     <subsystem name="gps" type="ublox"/>
   </firmware>
 
-<!-- **************************** MODULES ********************************** --> 
+<!-- **************************** MODULES ********************************** -->
   <modules>
 <!--    <load name="light.xml">
       <define name="LIGHT_LED_STROBE" value="3"/>
@@ -68,7 +68,7 @@
     </load>
   </modules>
 
-<!-- ***************************** SERVOS ********************************** --> 
+<!-- ***************************** SERVOS ********************************** -->
   <servos>
     <!-- Define here to which CONNECTOR NUMBER the servo is connected to, on the autopilot cicuit board -->
     <servo name="THROTTLE"      no="9" min="1000" neutral="1000" max="2000"/>
@@ -85,7 +85,7 @@
     <axis name="YAW"      failsafe_value="0"/>
     <axis name="PITCH"    failsafe_value="0"/>
 <!-- both ailerons up as braking spoilerons -->
-    <axis name="BRAKE"    failsafe_value="9600"/>  
+    <axis name="BRAKE"    failsafe_value="9600"/>
   </commands>
 
   <rc_commands>
@@ -167,7 +167,7 @@
 <!--    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/> -->
 
 <!-- Note that 3S lipo 3.1*3=9.3 most of the time the cutof for the motor part of the ESC -->
-    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>   
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
     <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
     <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
 
@@ -250,7 +250,7 @@
     <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
-<!-- ******************* VERTICAL CONTROL ********************************** --> 
+<!-- ******************* VERTICAL CONTROL ********************************** -->
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
     <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
     <!-- outer loop proportional gain -->
@@ -272,7 +272,7 @@
     <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
   </section>
 
-<!-- ******************* HORIZONTAL CONTROL ******************************** --> 
+<!-- ******************* HORIZONTAL CONTROL ******************************** -->
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="-1.16799998283"/>
diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 89e5020932..25687b7151 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -88,7 +88,7 @@ $(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_
 # I2C
 #
 include $(CFG_SHARED)/i2c_select.makefile
-	
+
 ######################################################################
 ##
 ## COMMON FOR ALL NON-SIMULATION TARGETS
diff --git a/conf/modules/gps_ubx_ucenter.xml b/conf/modules/gps_ubx_ucenter.xml
index 063d4bc866..883ecbd0d8 100644
--- a/conf/modules/gps_ubx_ucenter.xml
+++ b/conf/modules/gps_ubx_ucenter.xml
@@ -7,7 +7,7 @@
 	-configures all the messages, and the rates
 	-automatic baudrate detection
 
-	Warning: you still need to tell the driver 
+	Warning: you still need to tell the driver
 	a) which paparazzi uart you use
 	b) inside the ublox gps there are also many ports. the tiny/ppzgps use ublox_internal_port1 but if for instance you use a LS-SAM or I2C device you need to configure:
 
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 4375369c88..91988428bd 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -8,7 +8,7 @@
 #define I2C_DEBUG_LED
 
 /////////// DEBUGGING //////////////
-// TODO: remove this 
+// TODO: remove this
 
 
 #ifdef I2C_DEBUG_LED
@@ -53,7 +53,7 @@ static inline void LED_ERROR(uint8_t base, uint8_t nr)
   for (int i=0;i<(base+nr);i++)
   {
     LED1_ON();
-    LED1_OFF();    
+    LED1_OFF();
   }
   LED2_OFF();
 }
@@ -91,7 +91,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 // Bypassing the libSTM I2C functions to have more control over the reading of registers
 // e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
 
-// Referring to STM32 manual: 
+// Referring to STM32 manual:
 // -Doc ID 13902 Rev 11
 
 // Status Register 1
@@ -155,35 +155,35 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 2 Addr
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 3 BTF
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 4 ERROR
   if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // Anything?
   if (( SR1 + SR2) != 0x0000)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   LED1_OFF();
 
@@ -195,35 +195,35 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 2 Stop
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, CR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 3 Busy
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 4 Tra
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 5 Master
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
   LED1_OFF();
 
   LED1_ON();
@@ -233,24 +233,24 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 2 PE
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_PE, CR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 3 SWRESET
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_SWRST, CR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-    
+  LED2_OFF();
+
   LED1_OFF();
-  
+
 }
 #endif
 
@@ -304,7 +304,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   // Issue a new start
   regs->CR1 =  (I2C_CR1_BIT_START | I2C_CR1_BIT_PE);
   periph->status = I2CStartRequested;
-  
+
 
 #ifdef I2C_DEBUG_LED
   LED_SHOW_ACTIVE_BITS(regs);
@@ -414,7 +414,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-      
+
     // Schedule a Stop
     PPRZ_I2C_SEND_STOP(regs);
 
@@ -456,8 +456,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
     // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-      
-    // --- make absolutely sure this command is not delayed too much after the previous: 
+
+    // --- make absolutely sure this command is not delayed too much after the previous:
     __disable_irq();
     //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
@@ -582,7 +582,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // read the byte2 we had in the buffer (BTF means 2 bytes available)
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
-    
+
     // Ask for an interrupt to read the last byte (which is normally still busy now)
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
@@ -636,7 +636,7 @@ static inline void i2c_error(struct i2c_periph *periph);
 static inline void i2c_irq(struct i2c_periph *periph)
 {
 
-  /*	
+  /*
 	There are 7 possible reasons to get here:
 
 	If IT_EV_FEN
@@ -651,21 +651,21 @@ static inline void i2c_irq(struct i2c_periph *periph)
 	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
 
 	// Beware: using the buffered I2C has some interesting properties:
-	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
            in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
 	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
 	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
 	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
 	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
-        
-        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and 
-           a new start in one go. 
+
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
+           a new start in one go.
 
           -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
            This also means you must think more in advance and a transaction could be popped from the stack even before it is
            actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
            is known.
-	    
+
 	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
 
 	If IT_EV_FEN AND IT_EV_BUF
@@ -696,7 +696,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
 	  // Following are not used
 	  I2CReadingByte,
-	  I2CAddrWrSent,	
+	  I2CAddrWrSent,
 	  I2CAddrRdSent,
 	  I2CSendingLastByte,
 	  I2CReadingLastByte,
@@ -723,7 +723,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
   ///////////////////////////////////////////////////////////////////////////////////
   // Reading the status:
-  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Caution: this clears several flags and can start transmissions etc...
   // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
   //   the transmission of the byte is finished. At higher clock rates that can be
   //   quite fast: so we allow no other interrupt to be triggered in between
@@ -732,7 +732,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
   // Direct Access to the I2C Registers
   // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-  
+
 #ifdef I2C_DEBUG_LED
   LED1_ON();
   LED1_OFF();
@@ -762,7 +762,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
     // If we still get an interrupt but there are no more things to do
     // (which can happen if an event was sheduled just before a bus error occurs)
-    // then its easy: just stop: clear all interrupt generating bits  
+    // then its easy: just stop: clear all interrupt generating bits
 
     // Clear Running Events
     stmi2c_clear_pending_interrupts(regs);
@@ -775,7 +775,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
     periph->status = I2CIdle;
 
-    // There are no transactions anymore: 
+    // There are no transactions anymore:
     // furtheron we need a transaction pointer: so we are not allowed to continue
     return;
   }
@@ -830,7 +830,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
           break;
         default:
           ret = stmi2c_readmany(regs,periph, trans);
-          break;    
+          break;
       }
     }
     else // TxRx or Tx
@@ -866,7 +866,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
       periph->status = I2CIdle;
 
 
-      // if we have no more transaction to process, stop here 
+      // if we have no more transaction to process, stop here
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
 
@@ -890,7 +890,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
     }
     // RxTx -> Restart and do Rx part
-    else 
+    else
     {
       trans->type = I2CTransRx;
       periph->status = I2CStartRequested;
@@ -949,10 +949,10 @@ static inline void i2c_error(struct i2c_periph *periph)
 #endif
 
   return;
-  
+
 }
 
-  
+
 /*
   // Make sure the bus is free before resetting (p722)
   if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
@@ -1143,7 +1143,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
 }
 
 
-void i2c_event(void) 
+void i2c_event(void)
 {
   static uint32_t cnt = 0;
   I2C_TypeDef *regs;
@@ -1170,7 +1170,7 @@ void i2c_event(void)
 #endif
 
 #ifdef USE_I2C2
-  
+
 #ifdef I2C_DEBUG_LED
   if (cnt == 0)
   {
@@ -1205,7 +1205,7 @@ void i2c_event(void)
 
         __enable_irq();
   }
-#endif  
+#endif
 
 
   //if (i2c2.status == I2CIdle)
diff --git a/sw/airborne/arch/stm32/sys_time_hw.h b/sw/airborne/arch/stm32/sys_time_hw.h
index cc74f25896..9d88a69d8c 100644
--- a/sw/airborne/arch/stm32/sys_time_hw.h
+++ b/sw/airborne/arch/stm32/sys_time_hw.h
@@ -75,7 +75,7 @@ static inline void sys_time_usleep(uint32_t us) {
   {
     if (sys_time_period_elapsed) ready--;
     sys_time_periodic();
-  }  
+  }
 }
 
 #endif /* SYS_TIME_HW_H */
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 11b947b375..7a9b078caa 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -156,7 +156,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.buf[0] = 0x03;
     i2c_submit(&i2c2, &i2c_test1);
   break;
-  default: 
+  default:
     i2c_test1.slave_addr = 0x3C;
     i2c_test1.type = I2CTransTxRx;
     i2c_test1.len_r = 5;
@@ -167,7 +167,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
 }
 
 
-void event_i2c_abuse_test(void) 
+void event_i2c_abuse_test(void)
 {
   if (i2c_idle(&i2c1))
   {
@@ -204,7 +204,7 @@ void event_i2c_abuse_test(void)
 	  }
   }
 
-  
+
   if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
   {
 	    if (i2c_abuse_test_counter < 16)
@@ -231,7 +231,7 @@ void event_i2c_abuse_test(void)
 
 	    if (i2c_abuse_test_counter < 16)
 	    {
-	      i2c_abuse_send_transaction( i2c_abuse_test_counter ); 	 
+	      i2c_abuse_send_transaction( i2c_abuse_test_counter );
   	      LED_TOGGLE(5);
 	    }
   }
diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c
index 7abe6112bb..aa56a5395c 100644
--- a/sw/airborne/modules/gps/gps_ubx_ucenter.c
+++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c
@@ -325,14 +325,14 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t
 #undef GOT_PAYLOAD
 #include "downlink.h"
 
-static bool_t gps_ubx_ucenter_configure(uint8_t nr) 
+static bool_t gps_ubx_ucenter_configure(uint8_t nr)
 {
   // Store the reply of the last configuration step and reset
   if (nr < GPS_UBX_UCENTER_CONFIG_STEPS)
     gps_ubx_ucenter.replies[nr] = gps_ubx_ucenter.reply;
-  
+
   gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
-  
+
   switch (nr) {
   case 0:
     UbxSend_MON_GET_VER();
@@ -355,7 +355,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
     DOWNLINK_SEND_DEBUG(DefaultChannel,6,gps_ubx_ucenter.replies);
 
     //////////////////////////////////
-    // Actual configuration start   
+    // Actual configuration start
 
     // Use old baudrate to issue a baudrate change command
     gps_ubx_ucenter_config_port();
diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.h b/sw/airborne/modules/gps/gps_ubx_ucenter.h
index 50f4300afc..a14e149297 100644
--- a/sw/airborne/modules/gps/gps_ubx_ucenter.h
+++ b/sw/airborne/modules/gps/gps_ubx_ucenter.h
@@ -30,7 +30,7 @@
 /** U-Center Variables */
 #define GPS_UBX_UCENTER_CONFIG_STEPS	  17
 
-struct gps_ubx_ucenter_struct 
+struct gps_ubx_ucenter_struct
 {
   uint8_t status;
   uint8_t reply;
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
index 87dd9f519f..1c4c0e0b81 100644
--- a/sw/airborne/modules/ins/ins_chimu_spi.c
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -108,9 +108,9 @@ void ins_periodic_task( void )
 
   float gps_speed = 0;
 
-  if (gps.fix == GPS_FIX_3D) 
+  if (gps.fix == GPS_FIX_3D)
   {
-    gps_speed = gps.speed_3d/100.; 
+    gps_speed = gps.speed_3d/100.;
   }
   gps_speed = FloatSwap(gps_speed);
 

From d597b95bda5279be6443a91f0e59dc0b75ff451d Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 22 Nov 2011 22:57:31 +0100
Subject: [PATCH 54/74] Re-converge super-test-hack branch back to normal

---
 .../subsystems/fixedwing/autopilot.makefile   |   2 +-
 conf/modules/mag_hmc5843.xml                  |  19 ++
 sw/airborne/modules/sensors/mag_hmc5843.c     |  55 +++++
 sw/airborne/modules/sensors/mag_hmc5843.h     |  33 +++
 sw/airborne/peripherals/hmc5843.h             |   4 +-
 sw/ground_segment/misc/Makefile               |   6 +-
 sw/ground_segment/misc/ivy2serial.c           | 217 ------------------
 7 files changed, 111 insertions(+), 225 deletions(-)
 create mode 100644 conf/modules/mag_hmc5843.xml
 create mode 100644 sw/airborne/modules/sensors/mag_hmc5843.c
 create mode 100644 sw/airborne/modules/sensors/mag_hmc5843.h
 delete mode 100644 sw/ground_segment/misc/ivy2serial.c

diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 89e5020932..25687b7151 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -88,7 +88,7 @@ $(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_
 # I2C
 #
 include $(CFG_SHARED)/i2c_select.makefile
-	
+
 ######################################################################
 ##
 ## COMMON FOR ALL NON-SIMULATION TARGETS
diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml
new file mode 100644
index 0000000000..c8439d8e3e
--- /dev/null
+++ b/conf/modules/mag_hmc5843.xml
@@ -0,0 +1,19 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="sensors">
+  <!-- <depend conflict="ins" -->
+  <header>
+    <file name="mag_hmc5843.h"/>
+  </header>
+  <init fun="hmc5843_module_init()"/>
+  <periodic fun="hmc5843_module_periodic()" freq="60"/>
+  <event fun="hmc5843_module_event()"/>
+  <makefile>
+    <define name="USE_I2C" />
+    <file name="mag_hmc5843.c"/>
+    <file name="../../peripherals/hmc5843.c"/>
+    <define name="HMC5843_I2C_DEVICE" value="i2c0" />
+    <define name="USE_I2C0" />
+    <define name="HMC5843_NO_IRQ" />
+  </makefile>
+</module>
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
new file mode 100644
index 0000000000..cc3965c14d
--- /dev/null
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -0,0 +1,55 @@
+/*
+ * 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.
+ *
+ */
+#include "estimator.h"
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include <math.h>
+
+#include "../../peripherals/hmc5843.h"
+
+
+int32_t mag_x, mag_y, mag_z;
+bool_t mag_valid;
+
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+void hmc5843_module_init( void ) {
+  hmc5843_init();
+}
+
+void hmc5843_module_periodic ( void )
+{
+  hmc5843_periodic();
+  mag_x = hmc5843.data.value[0];
+  mag_y = hmc5843.data.value[1];
+  mag_z = hmc5843.data.value[2];
+  RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+}
+
+void hmc5843_module_event( void )
+{
+  hmc5843_idle_task();
+}
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h
new file mode 100644
index 0000000000..d21dbb5037
--- /dev/null
+++ b/sw/airborne/modules/sensors/mag_hmc5843.h
@@ -0,0 +1,33 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef HMC5843__H
+#define HMC5843__H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+extern int32_t mag_x, mag_y, mag_z;
+
+extern void hmc5843_module_init( void );
+extern void hmc5843_module_periodic( void );
+extern void hmc5843_module_event( void );
+
+#endif // HMC5843__H
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index f3cd97515b..ab71e07d80 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -30,6 +30,8 @@
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
     uint32_t timeout;
+    uint8_t sent_tx;
+    uint8_t sent_rx;
     uint8_t initialized;
     uint8_t data_available;
   union {
@@ -40,8 +42,6 @@ struct Hmc5843 {
 
 extern struct Hmc5843 hmc5843;
 
-#define HMC5843_USE_INT
-
 #ifndef HMC5843_NO_IRQ
 #include "peripherals/hmc5843_arch.h"
 
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
index bc0e6f8100..d7cf0118fb 100644
--- a/sw/ground_segment/misc/Makefile
+++ b/sw/ground_segment/misc/Makefile
@@ -7,7 +7,7 @@ else
 endif
 
 
-all: davis2ivy kestrel2ivy ivy2serial
+all: davis2ivy kestrel2ivy
 
 clean:
 	rm -f *.o davis2ivy kestrel2ivy
@@ -18,9 +18,5 @@ davis2ivy: davis2ivy.o
 kestrel2ivy: kestrel2ivy.o
 	g++ -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy
 
-ivy2serial: ivy2serial.o
-	g++ -o ivy2serial ivy2serial.o $(LIBRARYS) -livy	
-
-
 %.o : %.c
 	gcc -c -O2 -Wall -I /opt/local/include/  $<
diff --git a/sw/ground_segment/misc/ivy2serial.c b/sw/ground_segment/misc/ivy2serial.c
deleted file mode 100644
index e006499f82..0000000000
--- a/sw/ground_segment/misc/ivy2serial.c
+++ /dev/null
@@ -1,217 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <ctype.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <termios.h>
-
-#include <signal.h>
-
-#include <Ivy/ivy.h>
-#include <Ivy/ivyloop.h>
-#include <Ivy/timer.h>
-#include <Ivy/version.h>
-
-//////////////////////////////////////////////////////////////////////////////////
-// SETTINGS
-//////////////////////////////////////////////////////////////////////////////////
-
-// Serial Repeat Rate
-long delay = 1000;
-
-// local_uav Number
-unsigned char send_ac_id = 5;
-
-//////////////////////////////////////////////////////////////////////////////////
-// local_uav DATA
-//////////////////////////////////////////////////////////////////////////////////
-
-
-struct _uav_type_
-{
-  // Header
-  unsigned char header;
-
-  // Data
-  unsigned char ac_id;
-  short int phi, theta, psi;
-  int utm_east,utm_north,utm_z;
-  unsigned char utm_zone;
-  
-  // Footer
-  unsigned char footer;
-} 
-__attribute__((packed))
-
-local_uav, remote_uav;
-
-//////////////////////////////////////////////////////////////////////////////////
-// IVY Reader
-//////////////////////////////////////////////////////////////////////////////////
-
-
-static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[])
-{
-  unsigned char id = atoi(argv[0]);
-
-  if (id != send_ac_id)
-  {
-    printf("NEGLECT: %d\n",id);
-    return;
-  }
-
-/*
-   <message name="ATTITUDE" id="6">
-     <field name="phi"   type="float" unit="rad" alt_unit="deg"/>
-     <field name="psi"   type="float" unit="rad" alt_unit="deg"/>
-     <field name="theta" type="float" unit="rad" alt_unit="deg"/>
-   </message>
-*/
-
-  local_uav.ac_id = id;
-  local_uav.phi   = (short int) (atof(argv[1]) * 1000.0);
-  local_uav.theta = (short int) (atof(argv[3]) * 1000.0);
-  local_uav.psi   = (short int) (atof(argv[2]) * 1000.0);
-
-}
-
-static void on_Gps(IvyClientPtr app, void *user_data, int argc, char *argv[])
-{
-  unsigned char id = atoi(argv[0]);
-
-  if (id != send_ac_id)
-  {
-    printf("NEGLECT: %d\n",id);
-    return;
-  }
-
-/*
-   <message name="GPS" id="8">
-     <field name="mode"       type="uint8"  unit="byte_mask"/>
-     <field name="utm_east"   type="int32"  unit="cm" alt_unit="m"/>
-     <field name="utm_north"  type="int32"  unit="cm" alt_unit="m"/>
-     <field name="course"     type="int16"  unit="decideg" alt_unit="deg"/>
-     <field name="alt"        type="int32"  unit="mm" alt_unit="m"/>
-     <field name="speed"      type="uint16" unit="cm/s" alt_unit="m/s"/>
-     <field name="climb"      type="int16"  unit="cm/s" alt_unit="m/s"/>
-     <field name="week"       type="uint16" unit="weeks"/>
-     <field name="itow"       type="uint32" unit="ms"/>
-     <field name="utm_zone"   type="uint8"/>
-     <field name="gps_nb_err" type="uint8"/>
-   </message>
-*/
-
-  local_uav.ac_id = id;
-  local_uav.utm_east = atoi(argv[2]);
-  local_uav.utm_north = atoi(argv[3]);
-  local_uav.utm_z = atoi(argv[5]);
-  local_uav.utm_zone = atoi(argv[10]);
-
-  printf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi);
-  printf("GPS ac=%d %d %d %d %d\n",local_uav.ac_id, local_uav.utm_east, local_uav.utm_north, local_uav.utm_z, local_uav.utm_zone);
-
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// SERIAL PORT
-//////////////////////////////////////////////////////////////////////////////////
-
-// pointer
-int fd;
-
-/// Open
-void open_port(const char* device) {
-  fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
-  if (fd == -1) {
-    fprintf(stderr, "open_port: unable to open device %s - ", device);
-    perror(NULL);
-    exit(EXIT_FAILURE);
-  }
-  // setup connection options
-  struct termios options;
-
-  // get the current options
-  tcgetattr(fd, &options);
-
-  // set local mode, enable receiver, set comm. options:
-  // 8 data bits, 1 stop bit, no parity, 9600 Baud
-  options.c_cflag = CLOCAL | CREAD | CS8 | B9600;
-
-  // write options back to port
-  tcsetattr(fd, TCSANOW, &options);
-}
-
-void send_port(void)
-{
-  int bytes = write(fd, &local_uav, sizeof(local_uav));
-  printf("SENT: %d bytes\n",bytes);
-}
-
-void read_port(void)
-{
-}
-
-void close_port(void)
-{
-  close(fd);
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// TIMER
-//////////////////////////////////////////////////////////////////////////////////
-
-// Timer
-void handle_timer (TimerId id, void *data, unsigned long delta) {
-  printf("TIMER\n");
-  send_port();
-}
-
-TimerId tid;
-
-/// Handler for Ctrl-C, exits the main loop
-void sigint_handler(int sig) {
-  printf("\nCLEAN STOP\n");
-  IvyStop();
-  TimerRemove(tid);
-  close_port(); 
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// MAIN
-//////////////////////////////////////////////////////////////////////////////////
-
-int main ( int argc, char** argv) 
-{
-  int s = sizeof(local_uav);
- 
-  if (argc < 3)
-  {
-    printf("Use: ivy2serial ac_id serial_device\n");
-    return -1;
-  }
-  
-  send_ac_id = atoi(argv[1]);
-
-  printf("Listening to AC=%d, \nSending Size of Data = %d \n",send_ac_id, s);
-
-  // make Ctrl-C stop the main loop and clean up properly
-  signal(SIGINT, sigint_handler);
-
-  open_port(argv[2]);
-
-  // create timer (Ivy)
-  tid = TimerRepeatAfter (0, delay, handle_timer, 0);
-
-
-  IvyInit ("IVY <-> Serial", "IVY <-> Serial READY", NULL, NULL, NULL, NULL);
-  IvyBindMsg(on_Attitude, NULL, "^(\\S*) ATTITUDE (\\S*) (\\S*) (\\S*)");
-  IvyBindMsg(on_Gps, NULL, "^(\\S*) GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
-  IvyStart("127.255.255.255");
-  
-  IvyMainLoop ();
-
-  return 0;
-}
-

From e06da71f7d91d824d83a96882b9778511e8149c1 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 22 Nov 2011 23:03:34 +0100
Subject: [PATCH 55/74] Whitespaces

---
 .../arch/stm32/mcu_periph/i2c_arch.cdw.c      | 62 ++++++------
 .../stm32/mcu_periph/i2c_arch.cdw.nolib.c     | 62 ++++++------
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 98 +++++++++----------
 sw/airborne/boards/lisa_l/baro_board.c        |  2 +-
 4 files changed, 112 insertions(+), 112 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
index fd1ffb3e20..5a0f71c84d 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
@@ -5,7 +5,7 @@
 #include <stm32/flash.h>
 #include <stm32/misc.h>
 
-//#include "led.h" 
+//#include "led.h"
 
 /////////// DEBUGGING //////////////
 
@@ -53,7 +53,7 @@ static inline void LED_ERROR(uint8_t nr)
       LED2_OFF();
     else
       LED2_ON();
-    LED2_OFF();    
+    LED2_OFF();
   }
 }
 
@@ -131,12 +131,12 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
 }
 
-static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph) 
+static void PPRZ_I2C_START_NEXT_TRANSACTION(struct i2c_periph* periph)
 {
   /* if we have no more transaction to process, stop here */
   if (periph->trans_extract_idx == periph->trans_insert_idx)
   {
-    // Should we disable just in case? normally not. So if more interrupts are 
+    // Should we disable just in case? normally not. So if more interrupts are
     // triggered there is a problem and we want to know.
     // I2C_ITConfig(periph->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
     periph->status = I2CIdle;
@@ -165,9 +165,9 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
 {
   // Finish Current
   trans->status = _status;
- 
+
   // When finished successfully the I2C_FLAG_MLS will be cleared after the stop condition was issued.
-  // However: we do not need to wait for it to go the the next step, but if no stop condition was 
+  // However: we do not need to wait for it to go the the next step, but if no stop condition was
   // sent yet than we are still talking to the same slave...
   // When we are here all paths to this function with success have already issued a STOP, the others not.
   // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
@@ -175,7 +175,7 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
   {
     // TODO: we might need to do much more here: see reset functions of antoine...
     I2C_GenerateSTOP(periph->reg_addr, ENABLE);
-  }  
+  }
 
   // Jump to the next
   periph->trans_extract_idx++;
@@ -190,7 +190,7 @@ static inline void PPRZ_I2C_HAS_FINISHED(struct i2c_periph *periph, struct i2c_t
 
 static inline void i2c_event(struct i2c_periph *periph)
 {
-  // Referring to manual: 
+  // Referring to manual:
   // -Doc ID 13902 Rev 11
 
 
@@ -204,7 +204,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
 
-  /*	
+  /*
 	There are 7 possible reasons to get here:
 
 	If IT_EV_FEN
@@ -219,13 +219,13 @@ static inline void i2c_event(struct i2c_periph *periph)
 	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
 
 	// Beware: using the buffered I2C has some interesting properties:
-	  -when receiving BTF only occurs after the 2nd received byte: after the first byte is received it is 
+	  -when receiving BTF only occurs after the 2nd received byte: after the first byte is received it is
            in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
 	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost)
 	  -when transmitting, and writing a byte to WD, you instantly get a new TxE interrupt while the first is not
 	   transmitted yet. The byte was pushed to the I2C serializer and the buffer is ready for more. You can already
 	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
-	    
+
 	// Beware: the order in which Status is read determines how flags are cleared.
 
 	If IT_EV_FEN AND IT_EV_BUF
@@ -242,7 +242,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 	// -status register flags better correspond to reality, especially in case of I2C errors
 
 	enum I2CStatus {
-	  I2CIdle,			// Dummy: Actual I2C Peripheral idle detection is safer with the 
+	  I2CIdle,			// Dummy: Actual I2C Peripheral idle detection is safer with the
 					// hardware status register flag I2C_FLAG_BUSY.
 
 	  I2CStartRequested,		// EV5: used to differentiate S en Sr
@@ -254,7 +254,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 	  I2CAddrRdSent,		// have buffered sending, these states
 	  I2CSendingLastByte,		// do not correspond to the real state of the
 	  I2CReadingLastByte,		// STM I2C driver so they are not used
-	  I2CStopRequested,		
+	  I2CStopRequested,
 
 	  I2CComplete,			// Used to provide the result
 	  I2CFailed
@@ -275,7 +275,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   ///////////////////////////////////////////////////////////////////////////////////
   // Reading the status:
-  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Caution: this clears several flags and can start transmissions etc...
   // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
   //   the transmission of the byte is finished. At higher clock rates that can be
   //   quite fast: so we allow no other interrupt to be triggered in between
@@ -299,7 +299,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   ///////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////
-  // START: Start Condition in Master Mode: 
+  // START: Start Condition in Master Mode:
   // STM Manual Ev5
   if (event & I2C_FLAG_SB)
   {
@@ -324,7 +324,7 @@ static inline void i2c_event(struct i2c_periph *periph)
       I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
       I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
     }
-    // Problem: this problem need to be triggerd as if the 
+    // Problem: this problem need to be triggerd as if the
     // status was not OK then the buf size is also bad
     else
     {
@@ -336,7 +336,7 @@ static inline void i2c_event(struct i2c_periph *periph)
   ///////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////
   // TRANSMIT: Buffer Can accept the next byte for transmission
-  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt 
+  // --> this means we HAVE TO fill the buffer and/or disable buf interrupts (otherwise this interrupt
   //     will be triggered until a start/stop occurs which can be quite long = many spurrious interrupts)
   // STM Manual Ev8
   else if (event & I2C_FLAG_TXE) // only possible when TRA(nsmitter) and no flag tra/start/stop/addr
@@ -347,36 +347,36 @@ static inline void i2c_event(struct i2c_periph *periph)
           PPRZ_I2C_RESTART(periph);
     }
 
-    // Do we have more data? and there is buffer room? 
+    // Do we have more data? and there is buffer room?
     // (neglect BTF: if it was set it just means we were too slow)
     else if (periph->idx_buf < trans->len_w)
     {
       I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
       periph->idx_buf++;
       // Was this one the Last? -> Disable the buf interrupt (until next start) and wait for BTF
-      // -we could gain a bit of efficiency by already starting the next action but for 
+      // -we could gain a bit of efficiency by already starting the next action but for
       //  code-readability we will wait until the last tx-byte is sent
       if (periph->idx_buf >= trans->len_w)
       {
         I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
         // If this is followed by a restart: then we need to set the startbit to avoid extra interrupts.
-        if (trans->type != I2CTransTx) 
+        if (trans->type != I2CTransTx)
         {
         I2C_GenerateSTOP(periph->reg_addr, ENABLE);
         }
-      } 
+      }
     }
 
     else if (event & I2C_FLAG_BTF)
     {
       // Ready -> Stop
-      if (trans->type == I2CTransTx) 
+      if (trans->type == I2CTransTx)
       {
-        // STM Manual Ev8_2 
+        // STM Manual Ev8_2
         I2C_GenerateSTOP(periph->reg_addr, ENABLE);
         PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
       }
-      // Rx/Trans -> Restart: 
+      // Rx/Trans -> Restart:
       // Do not wait for BTF
     }
     // If we had no more data but got no BTF then there is a problem
@@ -392,12 +392,12 @@ static inline void i2c_event(struct i2c_periph *periph)
   // while receiving: the master needs to signal to the slave if more data is needed
   else if ((event & I2C_FLAG_ADDR) || (event & I2C_FLAG_RXNE))
   {
-    // data is available every time RXNE is set. If BTF is set it means that 2 bytes are 
+    // data is available every time RXNE is set. If BTF is set it means that 2 bytes are
     // ready to read (one in shift register) and I2C has stopped until the buffer can accept new data.
-    if (event & I2C_FLAG_RXNE) 
+    if (event & I2C_FLAG_RXNE)
     {
       uint8_t read_byte =  I2C_ReceiveData(periph->reg_addr);
-      if (periph->idx_buf < trans->len_r) 
+      if (periph->idx_buf < trans->len_r)
       {
         trans->buf[periph->idx_buf] = read_byte;
         periph->idx_buf++;
@@ -405,12 +405,12 @@ static inline void i2c_event(struct i2c_periph *periph)
     }
 
     // This last byte has arrived
-    if (periph->idx_buf >= trans->len_r) 
+    if (periph->idx_buf >= trans->len_r)
     {
       PPRZ_I2C_HAS_FINISHED(periph, trans, I2CTransSuccess);
     }
     // Tell the Slave it will be the last one
-    else if (periph->idx_buf >= trans->len_r-1) 
+    else if (periph->idx_buf >= trans->len_r-1)
     {
       I2C_AcknowledgeConfig(periph->reg_addr, DISABLE);         // give them a nack once it's done
       I2C_GenerateSTOP(periph->reg_addr, ENABLE);               // and follow with a stop
@@ -423,7 +423,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   }
 
-  // Now re-enable IRQ... it's been too long  
+  // Now re-enable IRQ... it's been too long
   // __enable_irq();
 
 
@@ -500,7 +500,7 @@ static inline void i2c_error(struct i2c_periph *periph)
 
 }
 
-  
+
 /*
   // Make sure the bus is free before resetting (p722)
   if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
index 25337d747b..852cd2d535 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
@@ -5,12 +5,12 @@
 #include <stm32/flash.h>
 #include <stm32/misc.h>
 
-//#include "led.h" 
+//#include "led.h"
 
 //#define I2C_DEBUG_LED
 
 /////////// DEBUGGING //////////////
-// TODO: remove this 
+// TODO: remove this
 
 
 #ifdef I2C_DEBUG_LED
@@ -54,7 +54,7 @@ static inline void LED_ERROR(uint8_t base, uint8_t nr)
   for (int i=0;i<(base+nr);i++)
   {
     LED1_ON();
-    LED1_OFF();    
+    LED1_OFF();
   }
   LED2_OFF();
 }
@@ -92,7 +92,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 // Bypassing the libSTM I2C functions to have more control over the reading of registers
 // e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
 
-// Referring to STM32 manual: 
+// Referring to STM32 manual:
 // -Doc ID 13902 Rev 11
 
 // Status Register 1
@@ -181,7 +181,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 
   // Issue a new start
   regs->CR1 |=  I2C_CR1_BIT_START;
-  
+
   // Enable Error IRQ, Event IRQ but disable Buffer IRQ
   regs->CR2 |= I2C_CR2_BIT_ITERREN;
   regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
@@ -200,29 +200,29 @@ static inline void LED_SHOW_ACTIVE_BITS(uint16_t SR1)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // Addr
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // BTF
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // ERROR
   if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
 
   LED1_OFF();
 }
@@ -329,7 +329,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-      
+
     // Schedule a Stop
     regs->CR1 |= I2C_CR1_BIT_STOP;
 
@@ -379,8 +379,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
     // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-      
-    // --- make absolutely sure this command is not delayed too much after the previous: 
+
+    // --- make absolutely sure this command is not delayed too much after the previous:
     __disable_irq();
     //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
@@ -519,7 +519,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // read the byte2 we had in the buffer (BTF means 2 bytes available)
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
-    
+
     // Ask for an interrupt to read the last byte (which is normally still busy now)
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
@@ -572,7 +572,7 @@ static inline void i2c_error(struct i2c_periph *periph);
 static inline void i2c_event(struct i2c_periph *periph)
 {
 
-  /*	
+  /*
 	There are 7 possible reasons to get here:
 
 	If IT_EV_FEN
@@ -587,21 +587,21 @@ static inline void i2c_event(struct i2c_periph *periph)
 	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
 
 	// Beware: using the buffered I2C has some interesting properties:
-	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
            in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
 	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
 	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
 	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
 	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
-        
-        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and 
-           a new start in one go. 
+
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
+           a new start in one go.
 
           -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
            This also means you must think more in advance and a transaction could be popped from the stack even before it is
            actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
            is known.
-	    
+
 	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
 
 	If IT_EV_FEN AND IT_EV_BUF
@@ -632,7 +632,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
 	  // Following are not used
 	  I2CReadingByte,
-	  I2CAddrWrSent,	
+	  I2CAddrWrSent,
 	  I2CAddrRdSent,
 	  I2CSendingLastByte,
 	  I2CReadingLastByte,
@@ -659,7 +659,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
   ///////////////////////////////////////////////////////////////////////////////////
   // Reading the status:
-  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Caution: this clears several flags and can start transmissions etc...
   // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
   //   the transmission of the byte is finished. At higher clock rates that can be
   //   quite fast: so we allow no other interrupt to be triggered in between
@@ -668,7 +668,7 @@ static inline void i2c_event(struct i2c_periph *periph)
   // Direct Access to the I2C Registers
   // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-  
+
 #ifdef I2C_DEBUG_LED
   LED1_ON();
   LED1_OFF();
@@ -748,7 +748,7 @@ static inline void i2c_event(struct i2c_periph *periph)
           break;
         default:
           ret = stmi2c_readmany(regs,periph, trans);
-          break;    
+          break;
       }
     }
     else // TxRx or Tx
@@ -794,7 +794,7 @@ static inline void i2c_event(struct i2c_periph *periph)
       if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
         periph->trans_extract_idx = 0;
 
-      // if we have no more transaction to process, stop here 
+      // if we have no more transaction to process, stop here
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
         periph->status = I2CIdle;
@@ -817,7 +817,7 @@ static inline void i2c_event(struct i2c_periph *periph)
 
     }
     // RxTx -> Restart and do Rx part
-    else 
+    else
     {
       trans->type = I2CTransRx;
       periph->status = I2CStartRequested;
@@ -876,10 +876,10 @@ static inline void i2c_error(struct i2c_periph *periph)
 #endif
 
   return;
-  
+
 }
 
-  
+
 /*
   // Make sure the bus is free before resetting (p722)
   if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 446f858a62..ddf079f151 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -8,7 +8,7 @@
 #define I2C_DEBUG_LED
 
 /////////// DEBUGGING //////////////
-// TODO: remove this 
+// TODO: remove this
 
 
 #ifdef I2C_DEBUG_LED
@@ -53,7 +53,7 @@ static inline void LED_ERROR(uint8_t base, uint8_t nr)
   for (int i=0;i<(base+nr);i++)
   {
     LED1_ON();
-    LED1_OFF();    
+    LED1_OFF();
   }
   LED2_OFF();
 }
@@ -91,7 +91,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 // Bypassing the libSTM I2C functions to have more control over the reading of registers
 // e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
 
-// Referring to STM32 manual: 
+// Referring to STM32 manual:
 // -Doc ID 13902 Rev 11
 
 // Status Register 1
@@ -137,7 +137,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 
 // Critical Zones
 
-#define __I2C_REG_CRITICAL_ZONE_START	
+#define __I2C_REG_CRITICAL_ZONE_START
 #define __I2C_REG_CRITICAL_ZONE_STOP
 
 
@@ -161,35 +161,35 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 2 Addr
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 3 BTF
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 4 ERROR
   if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // Anything?
   if (( SR1 + SR2) != 0x0000)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   LED1_OFF();
 
@@ -201,35 +201,35 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 2 Stop
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, CR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 3 Busy
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 4 Tra
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 5 Master
   if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
   LED1_OFF();
 
 //#define I2C_DEBUG_LED_CONTROL
@@ -243,24 +243,24 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
+  LED2_OFF();
 
   // 2 PE
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_PE, CR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-  
+  LED2_OFF();
+
   // 3 SWRESET
   if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_SWRST, CR1 ) )
     LED2_ON();
   else
     LED2_OFF();
-  LED2_OFF();    
-    
+  LED2_OFF();
+
   LED1_OFF();
-#endif  
+#endif
 
 }
 #endif
@@ -315,7 +315,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   // Issue a new start
   regs->CR1 =  (I2C_CR1_BIT_START | I2C_CR1_BIT_PE);
   periph->status = I2CStartRequested;
-  
+
 
 #ifdef I2C_DEBUG_LED
   LED_SHOW_ACTIVE_BITS(regs);
@@ -430,7 +430,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
 
     // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-      
+
     // Schedule a Stop
     PPRZ_I2C_SEND_STOP(regs);
 
@@ -476,8 +476,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
     // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-      
-    // --- make absolutely sure this command is not delayed too much after the previous: 
+
+    // --- make absolutely sure this command is not delayed too much after the previous:
     __I2C_REG_CRITICAL_ZONE_START;
     //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
@@ -606,7 +606,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // read the byte2 we had in the buffer (BTF means 2 bytes available)
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
-    
+
     // Ask for an interrupt to read the last byte (which is normally still busy now)
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
@@ -665,7 +665,7 @@ static inline void i2c_error(struct i2c_periph *periph);
 static inline void i2c_irq(struct i2c_periph *periph)
 {
 
-  /*	
+  /*
 	There are 7 possible reasons to get here:
 
 	If IT_EV_FEN
@@ -680,21 +680,21 @@ static inline void i2c_irq(struct i2c_periph *periph)
 	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
 
 	// Beware: using the buffered I2C has some interesting properties:
-	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is 
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
            in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
 	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
 	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
 	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
 	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
-        
-        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and 
-           a new start in one go. 
+
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
+           a new start in one go.
 
           -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
            This also means you must think more in advance and a transaction could be popped from the stack even before it is
            actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
            is known.
-	    
+
 	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
 
 	If IT_EV_FEN AND IT_EV_BUF
@@ -725,7 +725,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
 	  // Following are not used
 	  I2CReadingByte,
-	  I2CAddrWrSent,	
+	  I2CAddrWrSent,
 	  I2CAddrRdSent,
 	  I2CSendingLastByte,
 	  I2CReadingLastByte,
@@ -752,7 +752,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
   ///////////////////////////////////////////////////////////////////////////////////
   // Reading the status:
-  // - Caution: this clears several flags and can start transmissions etc... 
+  // - Caution: this clears several flags and can start transmissions etc...
   // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
   //   the transmission of the byte is finished. At higher clock rates that can be
   //   quite fast: so we allow no other interrupt to be triggered in between
@@ -761,7 +761,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
   // Direct Access to the I2C Registers
   // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-  
+
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   //
@@ -785,7 +785,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
     // If we still get an interrupt but there are no more things to do
     // (which can happen if an event was sheduled just before a bus error occurs)
-    // then its easy: just stop: clear all interrupt generating bits  
+    // then its easy: just stop: clear all interrupt generating bits
 
     // Count The Errors
     i2c_error(periph);
@@ -798,7 +798,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
     periph->status = I2CIdle;
 
-    // There are no transactions anymore: 
+    // There are no transactions anymore:
     // furtheron we need a transaction pointer: so we are not allowed to continue
     return;
   }
@@ -868,7 +868,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
           break;
         default:
           ret = stmi2c_readmany(regs,periph, trans);
-          break;    
+          break;
       }
     }
     else // TxRx or Tx
@@ -926,7 +926,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
       periph->status = I2CIdle;
 
 
-      // if we have no more transaction to process, stop here 
+      // if we have no more transaction to process, stop here
       if (periph->trans_extract_idx == periph->trans_insert_idx)
       {
 
@@ -950,7 +950,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
 
     }
     // RxTx -> Restart and do Rx part
-    else 
+    else
     {
       trans->type = I2CTransRx;
       periph->status = I2CStartRequested;
@@ -1009,10 +1009,10 @@ static inline void i2c_error(struct i2c_periph *periph)
 #endif
 
   return;
-  
+
 }
 
-  
+
 /*
   // Make sure the bus is free before resetting (p722)
   if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
@@ -1215,7 +1215,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
   LED2_OFF();
 #endif
         __enable_irq();
-void i2c_event(void) 
+void i2c_event(void)
 {
   static uint32_t cnt = 0;
   //I2C_TypeDef *regs;
@@ -1242,7 +1242,7 @@ void i2c_event(void)
 #endif
 
 #ifdef USE_I2C2
-  
+
 #ifdef I2C_DEBUG_LED
   if (cnt == 0)
   {
@@ -1277,7 +1277,7 @@ void i2c_event(void)
 
         __enable_irq();
   }
-#endif  
+#endif
 
 
   //if (i2c2.status == I2CIdle)
diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c
index 64a2b85b50..5fc013b62b 100644
--- a/sw/airborne/boards/lisa_l/baro_board.c
+++ b/sw/airborne/boards/lisa_l/baro_board.c
@@ -25,7 +25,7 @@ void baro_init(void) {
 
 
 void baro_periodic(void) {
-  return; 
+  return;
 
   switch (baro_board.status) {
   case LBS_UNINITIALIZED:

From 9c246b0aef60ad5a03de297103cc12938b393cdf Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 23 Nov 2011 09:45:16 +0100
Subject: [PATCH 56/74] merged version of I2C driver

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index ddf079f151..03e1ca9faa 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -613,6 +613,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
   }
   else // Hardware error
   {
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+#endif
     return STMI2C_SubTra_Error;
   }
 
@@ -639,7 +646,7 @@ static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
   if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
     // ADDR: Cleared by software when reading SR1 and then SR2
-    uint16_t SR2 __attribute__ ((unused)) = SR2;
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
   }
   // Byte Transfer Finished
   if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
@@ -762,6 +769,12 @@ static inline void i2c_irq(struct i2c_periph *periph)
   // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
+#ifdef I2C_DEBUG_LED
+  LED1_ON();
+  LED1_OFF();
+#endif
+
+
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   ///////////////////////////////////////////////////////////////////////////////////////////////////////
   //
@@ -891,8 +904,8 @@ static inline void i2c_irq(struct i2c_periph *periph)
         // Program a stop
         PPRZ_I2C_SEND_STOP(regs);
 
-        // Silent further BTF
-        regs->DR = 0;
+        // Silent any BTF that would occur before STOP is executed
+        regs->DR = 0x00;
       }
 
       // In case of unexpected condition: e.g. not slave, no event

From 1737b55862d253b00ae65ec856fefe89792b29b9 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 23 Nov 2011 09:48:12 +0100
Subject: [PATCH 57/74] cleanup

---
 sw/airborne/arch/stm32/mcu_periph/{ => obsolete}/i2c_arch.cdw.c   | 0
 .../arch/stm32/mcu_periph/{ => obsolete}/i2c_arch.cdw.nolib.c     | 0
 sw/airborne/arch/stm32/mcu_periph/{ => obsolete}/i2c_arch.old.c   | 0
 3 files changed, 0 insertions(+), 0 deletions(-)
 rename sw/airborne/arch/stm32/mcu_periph/{ => obsolete}/i2c_arch.cdw.c (100%)
 rename sw/airborne/arch/stm32/mcu_periph/{ => obsolete}/i2c_arch.cdw.nolib.c (100%)
 rename sw/airborne/arch/stm32/mcu_periph/{ => obsolete}/i2c_arch.old.c (100%)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.c
similarity index 100%
rename from sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.c
rename to sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.c
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.nolib.c
similarity index 100%
rename from sw/airborne/arch/stm32/mcu_periph/i2c_arch.cdw.nolib.c
rename to sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.nolib.c
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.old.c
similarity index 100%
rename from sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c
rename to sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.old.c

From 804b9f03afb3e87585fc77013c024a3f574cba68 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 23 Nov 2011 09:49:16 +0100
Subject: [PATCH 58/74] cleanup

---
 sw/airborne/arch/stm32/mcu_periph/{obsolete => }/i2c_arch.old.c | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename sw/airborne/arch/stm32/mcu_periph/{obsolete => }/i2c_arch.old.c (100%)

diff --git a/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.old.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c
similarity index 100%
rename from sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.old.c
rename to sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c

From 31cd96380d989f387b66a30f6dea8e9844378534 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 23 Nov 2011 09:58:08 +0100
Subject: [PATCH 59/74] order history

---
 .../{i2c_arch.cdw.c => i2c_attempt1.c}        |    0
 ..._arch.cdw.nolib.c => i2c_attempt2_nolib.c} |    0
 .../mcu_periph/obsolete/i2c_attempt3_subtra.c | 1286 +++++++++++++++++
 3 files changed, 1286 insertions(+)
 rename sw/airborne/arch/stm32/mcu_periph/obsolete/{i2c_arch.cdw.c => i2c_attempt1.c} (100%)
 rename sw/airborne/arch/stm32/mcu_periph/obsolete/{i2c_arch.cdw.nolib.c => i2c_attempt2_nolib.c} (100%)
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt3_subtra.c

diff --git a/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt1.c
similarity index 100%
rename from sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.c
rename to sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt1.c
diff --git a/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.nolib.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt2_nolib.c
similarity index 100%
rename from sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_arch.cdw.nolib.c
rename to sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt2_nolib.c
diff --git a/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt3_subtra.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt3_subtra.c
new file mode 100644
index 0000000000..91988428bd
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt3_subtra.c
@@ -0,0 +1,1286 @@
+#include "mcu_periph/i2c.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+
+#define I2C_DEBUG_LED
+
+/////////// DEBUGGING //////////////
+// TODO: remove this
+
+
+#ifdef I2C_DEBUG_LED
+
+static inline void LED1_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
+}
+
+static inline void LED1_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
+}
+
+static inline void LED2_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
+}
+
+static inline void LED2_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET );
+}
+
+static inline void LED_INIT(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  LED1_OFF();
+  LED2_OFF();
+}
+
+
+static inline void LED_ERROR(uint8_t base, uint8_t nr)
+{
+  LED2_ON();
+  for (int i=0;i<(base+nr);i++)
+  {
+    LED1_ON();
+    LED1_OFF();
+  }
+  LED2_OFF();
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef  I2C1_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 40000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef  I2C2_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+//      .I2C_ClockSpeed = 37000
+      .I2C_ClockSpeed = 400000
+};
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// Bypassing the libSTM I2C functions to have more control over the reading of registers
+// e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
+
+// Referring to STM32 manual:
+// -Doc ID 13902 Rev 11
+
+// Status Register 1
+
+#define I2C_SR1_BIT_SB			(1<<0)		// Start Condition Met
+#define I2C_SR1_BIT_ADDR		(1<<1)		// Address Sent
+#define I2C_SR1_BIT_BTF			(1<<2)		// SCL held low
+#define I2C_SR1_BIT_RXNE		(1<<6)		// Data Read Available
+#define I2C_SR1_BIT_TXE			(1<<7)		// TX buffer space available
+
+#define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop (usually interference)
+#define I2C_SR1_BIT_ERR_ARLO		(1<<9)		// Arbitration Lost (in multimaster) or SDA short-to-ground (in single master)
+#define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure (too fast/too soon/no sensor/wiring break/...)
+#define I2C_SR1_BIT_ERR_OVR		(1<<11)		// Overrun [data loss] (in slave) or SCL short-to-ground (in single master)
+
+#define I2C_SR1_BITS_ERR		((1<<8)|(1<<9)|(1<<10)|(1<<11)|(1<<12)|(1<<14)|(1<<15))
+
+// Status Register 2
+
+#define I2C_SR2_BIT_TRA			(1<<2)		// Transmitting
+#define I2C_SR2_BIT_BUSY		(1<<1)		// Busy
+#define I2C_SR2_BIT_MSL			(1<<0)		// Master Selected
+
+// Control Register 1
+
+#define I2C_CR1_BIT_PE			(1<<0)		// Peripheral Enable
+#define I2C_CR1_BIT_START		(1<<8)		// Generate a Start
+#define I2C_CR1_BIT_STOP		(1<<9)		// Generate a Stop
+#define I2C_CR1_BIT_ACK			(1<<10)		// ACK / NACK
+#define I2C_CR1_BIT_POS			(1<<11)		// Ack will control not the next but secondnext received byte
+#define I2C_CR1_BIT_SWRST		(1<<15)		// Clear Busy Condition when no stop was detected
+
+// Control Register 2
+
+#define I2C_CR2_BIT_ITERREN		(1<<8)		// Error Interrupt
+#define I2C_CR2_BIT_ITEVTEN		(1<<9)		// Event Interrupt
+#define I2C_CR2_BIT_ITBUFEN		(1<<10)		// Buffer Interrupt
+
+
+// Bit Control
+
+#define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// TODO: remove debug
+#ifdef I2C_DEBUG_LED
+
+static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
+{
+  uint16_t CR1 = regs->CR1;
+  uint16_t SR1 = regs->SR1;
+  uint16_t SR2 = regs->SR2;
+  // Note: reading SR1 and then SR2 will clear ADDR bits
+
+  LED1_ON();
+
+  // 1 Start
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 2 Addr
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 3 BTF
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 4 ERROR
+  if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // Anything?
+  if (( SR1 + SR2) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  LED1_OFF();
+
+
+  LED1_ON();
+
+  // 1 Start
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_START, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 2 Stop
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 3 Busy
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 4 Tra
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 5 Master
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+  LED1_OFF();
+
+  LED1_ON();
+
+  // 1 Anything CR?
+  if (( CR1) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 2 PE
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_PE, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 3 SWRESET
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_SWRST, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  LED1_OFF();
+
+}
+#endif
+
+static inline void PPRZ_I2C_SEND_STOP(I2C_TypeDef *regs)
+{
+  // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+  regs->CR1 |= I2C_CR1_BIT_STOP;
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+}
+
+// (RE)START
+
+static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  periph->idx_buf = 0;
+
+#ifdef I2C_DEBUG_LED
+  LED_SHOW_ACTIVE_BITS(regs);
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+
+#endif
+
+/*
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
+  {
+    regs->CR1 &= ~ I2C_CR1_BIT_STOP;
+  }
+*/
+
+  // Enable Error IRQ, Event IRQ but disable Buffer IRQ
+  regs->CR2 |= I2C_CR2_BIT_ITERREN;
+  regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+  // Issue a new start
+  regs->CR1 =  (I2C_CR1_BIT_START | I2C_CR1_BIT_PE);
+  periph->status = I2CStartRequested;
+
+
+#ifdef I2C_DEBUG_LED
+  LED_SHOW_ACTIVE_BITS(regs);
+#endif
+}
+
+// STOP
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+//	SUBTRANSACTION SEQUENCES
+
+enum STMI2CSubTransactionStatus {
+  STMI2C_SubTra_Busy,
+  STMI2C_SubTra_Ready_StopRequested,
+  STMI2C_SubTra_Ready
+};
+
+static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // Disable buffer interrupt
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Send Slave address and wait for ADDR interrupt
+    regs->DR = trans->slave_addr;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // Now read SR2 to clear the ADDR
+    uint16_t SR2  __attribute__ ((unused)) = regs->SR2;
+
+    // Maybe check we are transmitting (did not loose arbitration for instance)
+    // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
+
+    // Send First max 2 bytes
+    regs->DR = trans->buf[0];
+    if (trans->len_w > 1)
+    {
+      regs->DR = trans->buf[1];
+      periph->idx_buf = 2;
+    }
+    else
+    {
+      periph->idx_buf = 1;
+    }
+
+    // Enable buffer-space available interrupt
+    // only if there is more to send: wait for TXE, no more to send: wait for BTF
+    if ( periph->idx_buf < trans->len_w)
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  // The buffer is not full anymore AND we were not waiting for BTF
+  else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
+  {
+    // Send the next byte
+    regs->DR = trans->buf[periph->idx_buf];
+    periph->idx_buf++;
+
+    // All bytes Sent? Then wait for BTF instead
+    if ( periph->idx_buf >= trans->len_w)
+    {
+      // Not interested anymore to know the buffer has space left
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+      // Next interrupt will be BTF (or error)
+    }
+  }
+  // BTF: means last byte was sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+      if (trans->type == I2CTransTx)
+      {
+        // Tell the driver we are ready
+        trans->status = I2CTransSuccess;
+      }
+
+      return STMI2C_SubTra_Ready;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // First Clear the ACK bit: after the next byte we do not want new bytes
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // --- next to steps MUST be executed together to avoid missing the stop
+    __disable_irq();
+
+    // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+
+    // Schedule a Stop
+    PPRZ_I2C_SEND_STOP(regs);
+
+    __enable_irq();
+    // --- end of critical zone -----------
+
+    // Enable the RXNE to get the result
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    trans->buf[0] = regs->DR;
+
+    // We got all the results (stop condition might still be in progress but this is the last interrupt)
+    trans->status = I2CTransSuccess;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    regs->CR1 |= I2C_CR1_BIT_POS;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    // clearing ACK after the byte transfer has already started will NACK the next (2nd)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+
+    // --- make absolutely sure this command is not delayed too much after the previous:
+    __disable_irq();
+    //       if transfer of DR was finished already then we will get too many bytes
+    // NOT First Clear the ACK bit but only AFTER clearing ADDR
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Disable the RXNE and wait for BTF
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    __enable_irq();
+    // --- end of critical zone -----------
+  }
+  // Receive buffer if full, master is halted: BTF
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Stop condition MUST be set BEFORE reading the DR
+    // otherwise since there is new buffer space a new byte will be read
+    PPRZ_I2C_SEND_STOP(regs);
+
+    trans->buf[0] = regs->DR;
+    trans->buf[1] = regs->DR;
+
+    // We got all the results
+    trans->status = I2CTransSuccess;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // The first data byte will be acked in read many so the slave knows it should send more
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    // Clear the SB flag
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    periph->idx_buf = 0;
+
+    // Enable RXNE: receive an interrupt any time a byte is available
+    // only enable if MORE than 3 bytes need to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+
+    // ACK is still on to get more DATA
+    // Read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // one or more bytes are available AND we were interested in Buffer interrupts
+  else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
+  {
+    // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+    }
+    // from : 3bytes -> last byte: do nothing
+    //
+    // finally: this was the last byte
+    else if (periph->idx_buf >= (trans->len_r - 1))
+    {
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+      // Last Value
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+
+      // We got all the results
+      trans->status = I2CTransSuccess;
+
+      return STMI2C_SubTra_Ready_StopRequested;
+    }
+
+    // Check for end of transaction: start waiting for BTF instead of RXNE
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+    else // idx >= len-3: there are 3 bytes to be read
+    {
+      // We want to halt I2C to have sufficient time to clear ACK, so:
+      // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer
+      // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted)
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    }
+  }
+  // Buffer is full while this was not a RXNE interrupt
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Now the shift register and data register contain data(n-2) and data(n-1)
+    // And I2C is halted so we have time
+
+    // --- Make absolutely sure the next 2 I2C actions are performed with no delay
+    __disable_irq();
+
+    // First we clear the ACK while the SCL is held low by BTF
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+
+    // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
+    PPRZ_I2C_SEND_STOP(regs);
+
+    __enable_irq();
+    // --- end of critical zone -----------
+
+    // read the byte2 we had in the buffer (BTF means 2 bytes available)
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+
+    // Ask for an interrupt to read the last byte (which is normally still busy now)
+    // The last byte will be received with RXNE
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else
+  {
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+#endif
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+
+static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
+{
+  uint16_t SR1 = regs->SR1;
+
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
+
+
+  // Start Condition Was Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // SB: cleared by software when reading SR1 and writing to DR
+    regs->DR = 0x00;
+  }
+  // Address Was Sent
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // ADDR: Cleared by software when reading SR1 and then SR2
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // Byte Transfer Finished
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // SB: cleared by software when reading SR1 and reading/writing to DR
+    uint8_t dummy __attribute__ ((unused)) = regs->DR;
+    regs->DR = 0x00;
+  }
+}
+
+
+static inline void i2c_error(struct i2c_periph *periph);
+
+static inline void i2c_irq(struct i2c_periph *periph)
+{
+
+  /*
+	There are 7 possible reasons to get here:
+
+	If IT_EV_FEN
+	-------------------------
+
+	We are always interested in all IT_EV_FEV: all are required.
+
+	1) SB		// Start Condition Success in Master mode
+	2) ADDR		// Address sent received Acknoledge
+	[3 ADDR10]	// -- 10bit address stuff
+	[4 STOPF]	// -- only for slaves: master has no stop interrupt
+	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
+
+	// Beware: using the buffered I2C has some interesting properties:
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
+           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
+	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
+	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
+           a new start in one go.
+
+          -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
+           This also means you must think more in advance and a transaction could be popped from the stack even before it is
+           actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
+           is known.
+
+	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
+
+	If IT_EV_FEN AND IT_EV_BUF
+	--------------------------
+
+	Buffer event are not always wanted and are tipically switched on during longer data transfers. Make sure to turn off in time.
+
+	6) RxNE
+	7) TxE
+
+	--------------------------------------------------------------------------------------------------
+	// This driver uses only a subset of the pprz_i2c_states for several reasons:
+	// -we have less interrupts than the I2CStatus states (for efficiency)
+	// -STM32 has such a powerfull I2C engine with plenty of status register flags that
+            only little extra status information needs to be stored.
+
+       // Status is re-used (abused) to remember the last COMMAND THAT WAS SENT to the STM I2C hardware.
+
+// TODO: check which are used
+	enum I2CStatus {
+	  I2CIdle,			// No more last command
+
+	  I2CStartRequested,		// Last command was start
+	  I2CRestartRequested,		// Last command was restart
+	  I2CStopRequested,		// Very important to not send double stop conditions
+
+	  I2CSendingByte,		// Some address/data operation
+
+	  // Following are not used
+	  I2CReadingByte,
+	  I2CAddrWrSent,
+	  I2CAddrRdSent,
+	  I2CSendingLastByte,
+	  I2CReadingLastByte,
+	  I2CComplete,
+	  I2CFailed
+	};
+
+	---------
+
+	The STM waits indefinately (holding SCL low) for user interaction:
+	a) after a master-start (waiting for address)
+	b) after an address (waiting for data)
+	   not during data sending when using buffered
+	c) after the last byte is transmitted (waiting for either stop or restart)
+	   not during data receiving when using buffered
+	   not after the last byte is received
+
+	-The STM I2C stalls indefinately when a stop condition was attempted that
+	did not succeed. The BUSY flag remains on.
+        -There is no STOP interrupt: use needs another way to finish.
+
+   */
+
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  // Reading the status:
+  // - Caution: this clears several flags and can start transmissions etc...
+  // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
+  //   the transmission of the byte is finished. At higher clock rates that can be
+  //   quite fast: so we allow no other interrupt to be triggered in between
+  //   reading the status and setting all needed flags
+
+  // Direct Access to the I2C Registers
+  // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+#ifdef I2C_DEBUG_LED
+  LED1_ON();
+  LED1_OFF();
+#endif
+
+
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  //
+  //	TRANSACTION HANDLER
+
+  enum STMI2CSubTransactionStatus ret = 0;
+
+  ///////////////////////
+  // Nothing Left To Do
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+
+    // no transaction and also an error?
+    LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+    // If we still get an interrupt but there are no more things to do
+    // (which can happen if an event was sheduled just before a bus error occurs)
+    // then its easy: just stop: clear all interrupt generating bits
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
+    // Count The Errors
+    i2c_error(periph);
+
+    // Mark this as a special error
+    periph->errors->unexpected_event_cnt++;
+
+    periph->status = I2CIdle;
+
+    // There are no transactions anymore:
+    // furtheron we need a transaction pointer: so we are not allowed to continue
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+
+  ///////////////////////////
+  // If there was an error:
+  if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+  {
+
+#ifdef I2C_DEBUG_LED
+        LED1_ON();
+        LED2_ON();
+	LED1_OFF();
+	LED2_OFF();
+
+        LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+    // Set result in transaction
+    trans->status = I2CTransFailed;
+
+    // Prepare for next
+    ret = STMI2C_SubTra_Ready;
+
+    // Make sure a TxRx does not Restart
+    trans->type = I2CTransRx;
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
+    // Count The Errors
+    i2c_error(periph);
+  }
+
+
+  ///////////////////////////
+  // Normal Event:
+  else
+  {
+
+    if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
+    {
+      switch (trans->len_r)
+      {
+        case 1:
+          ret = stmi2c_read1(regs,trans);
+          break;
+        case 2:
+          ret = stmi2c_read2(regs,trans);
+          break;
+        default:
+          ret = stmi2c_readmany(regs,periph, trans);
+          break;
+      }
+    }
+    else // TxRx or Tx
+    {
+      ret = stmi2c_send(regs,periph,trans);
+    }
+  }
+
+  /////////////////////////////////
+  // Sub-transaction has finished
+  if (ret != STMI2C_SubTra_Busy)
+  {
+    // If a restart is not needed
+    if (trans->type != I2CTransTxRx)
+    {
+      // Ready, no stop condition set yet
+      if (ret == STMI2C_SubTra_Ready)
+      {
+
+        // Program a stop
+        PPRZ_I2C_SEND_STOP(regs);
+
+        // Silent any BTF that would occur before STOP is executed
+        regs->DR = 0x00;
+      }
+
+      // Jump to the next transaction
+      periph->trans_extract_idx++;
+      if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+        periph->trans_extract_idx = 0;
+
+      // Tell everyone we are ready
+      periph->status = I2CIdle;
+
+
+      // if we have no more transaction to process, stop here
+      if (periph->trans_extract_idx == periph->trans_insert_idx)
+      {
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+      }
+      // if not, start next transaction
+      else
+      {
+        // Restart transaction doing the Rx part now
+// --- moved to idle function
+        PPRZ_I2C_SEND_START(periph);
+// ------
+     }
+
+    }
+    // RxTx -> Restart and do Rx part
+    else
+    {
+      trans->type = I2CTransRx;
+      periph->status = I2CStartRequested;
+      regs->CR1 |= I2C_CR1_BIT_START;
+
+      // Silent any BTF that would occur before SB
+      regs->DR = 0x00;
+    }
+  }
+
+  return;
+}
+
+static inline void i2c_error(struct i2c_periph *periph)
+{
+  uint8_t err_nr = 0;
+  periph->errors->er_irq_cnt;
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
+    periph->errors->ack_fail_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_AF);
+    err_nr = 1;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
+    periph->errors->miss_start_stop_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BERR);
+    err_nr = 2;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
+    periph->errors->arb_lost_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_ARLO);
+    err_nr = 3;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
+    periph->errors->over_under_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_OVR);
+    err_nr = 4;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
+    periph->errors->pec_recep_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_PECERR);
+    err_nr = 5;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
+    periph->errors->timeout_tlow_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_TIMEOUT);
+    err_nr = 6;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+    periph->errors->smbus_alert_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_SMBALERT);
+    err_nr = 7;
+  }
+
+#ifdef I2C_DEBUG_LED
+  LED_ERROR(20, err_nr);
+#endif
+
+  return;
+
+}
+
+
+/*
+  // Make sure the bus is free before resetting (p722)
+  if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
+    // Reset the I2C block
+    I2C_SoftwareResetCmd(periph->reg_addr, ENABLE);
+    I2C_SoftwareResetCmd(periph->reg_addr, DISABLE);
+  }
+*/
+
+//#endif /* USE_I2C2 */
+
+
+
+
+#ifdef USE_I2C1
+
+struct i2c_errors i2c1_errors;
+
+void i2c1_hw_init(void) {
+
+  i2c1.reg_addr = I2C1;
+  i2c1.init_struct = &I2C1_InitStruct;
+  i2c1.scl_pin = GPIO_Pin_6;
+  i2c1.sda_pin = GPIO_Pin_7;
+  i2c1.errors = &i2c1_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c1_errors);
+
+  // Extra
+#ifdef I2C_DEBUG_LED
+  LED_INIT();
+#else
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C1);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C1 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C1 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C1 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c1.scl_pin | i2c1.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C1);
+
+  // enable peripheral
+  I2C_Cmd(I2C1, ENABLE);
+
+  I2C_Init(I2C1, i2c1.init_struct);
+
+  // enable error interrupts
+  I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE);
+
+#endif
+}
+
+void i2c1_ev_irq_handler(void) {
+  i2c_irq(&i2c1);
+}
+
+void i2c1_er_irq_handler(void) {
+  i2c_irq(&i2c1);
+}
+
+#endif /* USE_I2C1 */
+
+#ifdef USE_I2C2
+
+struct i2c_errors i2c2_errors;
+
+void i2c2_hw_init(void) {
+
+  i2c2.reg_addr = I2C2;
+  i2c2.init_struct = &I2C2_InitStruct;
+  i2c2.scl_pin = GPIO_Pin_10;
+  i2c2.sda_pin = GPIO_Pin_11;
+  i2c2.errors = &i2c2_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c2_errors);
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C2);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C2 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C2 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C2 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c2.scl_pin | i2c2.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C2);
+
+  // enable peripheral
+  I2C_Cmd(I2C2, ENABLE);
+
+  I2C_Init(I2C2, i2c2.init_struct);
+
+  // enable error interrupts
+  I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
+
+}
+
+
+
+void i2c2_ev_irq_handler(void) {
+  i2c_irq(&i2c2);
+}
+
+void i2c2_er_irq_handler(void) {
+  i2c_irq(&i2c2);
+}
+
+#endif /* USE_I2C2 */
+
+
+void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
+{
+  if (i2c_idle(periph))
+  {
+    if (periph == &i2c2)
+    {
+      I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+      I2C_Init(I2C2, i2c2.init_struct);
+    }
+
+#ifdef I2C_DEBUG_LED
+        __disable_irq();
+
+        LED2_ON();
+        LED1_ON();
+        LED2_OFF();
+        LED1_OFF();
+        LED2_ON();
+        LED1_ON();
+        LED2_OFF();
+        LED1_OFF();
+
+        __enable_irq();
+#endif
+
+   }
+}
+
+
+void i2c_event(void)
+{
+  static uint32_t cnt = 0;
+  I2C_TypeDef *regs;
+  cnt++;
+  if (cnt > 10000) cnt = 0;
+
+#ifndef I2C_DEBUG_LED
+#ifdef USE_I2C1
+  if (i2c1.status == I2CIdle)
+  {
+    if (i2c_idle(&i2c1))
+    {
+      __disable_irq();
+      // More work to do
+      if (i2c1.trans_extract_idx != i2c1.trans_insert_idx)
+      {
+        // Restart transaction doing the Rx part now
+        PPRZ_I2C_SEND_START(&i2c1);
+      }
+      __enable_irq();
+    }
+  }
+#endif
+#endif
+
+#ifdef USE_I2C2
+
+#ifdef I2C_DEBUG_LED
+  if (cnt == 0)
+  {
+        __disable_irq();
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        if (i2c2.status == I2CIdle)
+        {
+          LED1_ON();
+   	  LED1_OFF();
+        }
+        else if (i2c2.status == I2CStartRequested)
+        {
+          LED1_ON();
+   	  LED1_OFF();
+          LED1_ON();
+   	  LED1_OFF();
+
+        }
+	LED2_OFF();
+
+        //regs = (I2C_TypeDef *) i2c2.reg_addr;
+        //LED_SHOW_ACTIVE_BITS(regs);
+
+        __enable_irq();
+  }
+#endif
+
+
+  //if (i2c2.status == I2CIdle)
+  {
+    //if (i2c_idle(&i2c2))
+    {
+      //__disable_irq();
+      // More work to do
+      //if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
+      {
+        // Restart transaction doing the Rx part now
+        //PPRZ_I2C_SEND_START(&i2c2);
+      }
+      //__enable_irq();
+    }
+  }
+#endif
+}
+
+/////////////////////////////////////////////////////////
+// Implement Interface Functions
+
+bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
+
+  uint8_t temp;
+  temp = periph->trans_insert_idx + 1;
+  if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
+  if (temp == periph->trans_extract_idx)
+    return FALSE;                          // queue full
+
+  t->status = I2CTransPending;
+
+  __disable_irq();
+  /* put transacation in queue */
+  periph->trans[periph->trans_insert_idx] = t;
+  periph->trans_insert_idx = temp;
+
+  /* if peripheral is idle, start the transaction */
+  // if (PPRZ_I2C_IS_IDLE(p))
+  if (periph->status == I2CIdle)
+  {
+    //if (i2c_idle(periph))
+    {
+#ifdef I2C_DEBUG_LED
+	if (periph == &i2c1)
+	{
+
+        }
+        else
+#endif
+        {
+          PPRZ_I2C_SEND_START(periph);
+        }
+    }
+  }
+  /* else it will be started by the interrupt handler when the previous transactions completes */
+  __enable_irq();
+
+  return TRUE;
+}
+
+bool_t i2c_idle(struct i2c_periph* periph)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+#ifdef I2C_DEBUG_LED
+	if (periph == &i2c1)
+	{
+	  return TRUE;
+        }
+#endif
+  if (periph->status == I2CIdle)
+    return ! (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, regs->SR2 ) );
+  else
+    return FALSE;
+}
+
+

From 820f0ca5692c28d474a177f536789d3f65b60c42 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 23 Nov 2011 10:51:13 +0100
Subject: [PATCH 60/74] new attempt: we now know there are 2 interrupts
 simultaneously ...

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     |   13 -
 .../obsolete/i2c_attempt4_nodouble_isr.c      | 1358 +++++++++++++++++
 sw/airborne/mcu_periph/i2c.h                  |    1 -
 3 files changed, 1358 insertions(+), 14 deletions(-)
 create mode 100644 sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt4_nodouble_isr.c

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 03e1ca9faa..8fb1a4da48 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1215,19 +1215,6 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
    }
 }
 
-
-        __disable_irq();
-#ifdef I2C_DEBUG_LED
-  LED1_ON();
-  LED1_OFF();
-#endif
-        __enable_irq();
-        __disable_irq();
-#ifdef I2C_DEBUG_LED
-  LED2_ON();
-  LED2_OFF();
-#endif
-        __enable_irq();
 void i2c_event(void)
 {
   static uint32_t cnt = 0;
diff --git a/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt4_nodouble_isr.c b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt4_nodouble_isr.c
new file mode 100644
index 0000000000..8fb1a4da48
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/obsolete/i2c_attempt4_nodouble_isr.c
@@ -0,0 +1,1358 @@
+#include "mcu_periph/i2c.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+
+#define I2C_DEBUG_LED
+
+/////////// DEBUGGING //////////////
+// TODO: remove this
+
+
+#ifdef I2C_DEBUG_LED
+
+static inline void LED1_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET );
+}
+
+static inline void LED1_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET );
+}
+
+static inline void LED2_ON(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET );
+}
+
+static inline void LED2_OFF(void)
+{
+  GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET );
+}
+
+static inline void LED_INIT(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  LED1_OFF();
+  LED2_OFF();
+}
+
+
+static inline void LED_ERROR(uint8_t base, uint8_t nr)
+{
+  LED2_ON();
+  for (int i=0;i<(base+nr);i++)
+  {
+    LED1_ON();
+    LED1_OFF();
+  }
+  LED2_OFF();
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef  I2C1_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+      .I2C_ClockSpeed = 40000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef  I2C2_InitStruct = {
+      .I2C_Mode = I2C_Mode_I2C,
+      .I2C_DutyCycle = I2C_DutyCycle_2,
+      .I2C_OwnAddress1 = 0x00,
+      .I2C_Ack = I2C_Ack_Enable,
+      .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+//      .I2C_ClockSpeed = 37000
+      .I2C_ClockSpeed = 400000
+};
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// Bypassing the libSTM I2C functions to have more control over the reading of registers
+// e.g. SR1 and SR2 should not always be read together as it might unwantedly clear ADDR flags etc.
+
+// Referring to STM32 manual:
+// -Doc ID 13902 Rev 11
+
+// Status Register 1
+
+#define I2C_SR1_BIT_SB			(1<<0)		// Start Condition Met
+#define I2C_SR1_BIT_ADDR		(1<<1)		// Address Sent
+#define I2C_SR1_BIT_BTF			(1<<2)		// SCL held low
+#define I2C_SR1_BIT_RXNE		(1<<6)		// Data Read Available
+#define I2C_SR1_BIT_TXE			(1<<7)		// TX buffer space available
+
+#define I2C_SR1_BIT_ERR_BUS		(1<<8)		// Misplaced Start/Stop (usually interference)
+#define I2C_SR1_BIT_ERR_ARLO		(1<<9)		// Arbitration Lost (in multimaster) or SDA short-to-ground (in single master)
+#define I2C_SR1_BIT_ERR_AF		(1<<10)		// Ack Failure (too fast/too soon/no sensor/wiring break/...)
+#define I2C_SR1_BIT_ERR_OVR		(1<<11)		// Overrun [data loss] (in slave) or SCL short-to-ground (in single master)
+
+#define I2C_SR1_BITS_ERR		((1<<8)|(1<<9)|(1<<10)|(1<<11)|(1<<12)|(1<<14)|(1<<15))
+
+// Status Register 2
+
+#define I2C_SR2_BIT_TRA			(1<<2)		// Transmitting
+#define I2C_SR2_BIT_BUSY		(1<<1)		// Busy
+#define I2C_SR2_BIT_MSL			(1<<0)		// Master Selected
+
+// Control Register 1
+
+#define I2C_CR1_BIT_PE			(1<<0)		// Peripheral Enable
+#define I2C_CR1_BIT_START		(1<<8)		// Generate a Start
+#define I2C_CR1_BIT_STOP		(1<<9)		// Generate a Stop
+#define I2C_CR1_BIT_ACK			(1<<10)		// ACK / NACK
+#define I2C_CR1_BIT_POS			(1<<11)		// Ack will control not the next but secondnext received byte
+#define I2C_CR1_BIT_SWRST		(1<<15)		// Clear Busy Condition when no stop was detected
+
+// Control Register 2
+
+#define I2C_CR2_BIT_ITERREN		(1<<8)		// Error Interrupt
+#define I2C_CR2_BIT_ITEVTEN		(1<<9)		// Event Interrupt
+#define I2C_CR2_BIT_ITBUFEN		(1<<10)		// Buffer Interrupt
+
+
+// Bit Control
+
+#define BIT_X_IS_SET_IN_REG(X,REG)	(((REG) & (X)) == (X))
+
+// Critical Zones
+
+#define __I2C_REG_CRITICAL_ZONE_START
+#define __I2C_REG_CRITICAL_ZONE_STOP
+
+
+//////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////
+
+// TODO: remove debug
+#ifdef I2C_DEBUG_LED
+
+static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs)
+{
+  uint16_t CR1 = regs->CR1;
+  uint16_t SR1 = regs->SR1;
+  uint16_t SR2 = regs->SR2;
+  // Note: reading SR1 and then SR2 will clear ADDR bits
+
+  LED1_ON();
+
+  // 1 Start
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 2 Addr
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 3 BTF
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 4 ERROR
+  if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // Anything?
+  if (( SR1 + SR2) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  LED1_OFF();
+
+
+  LED1_ON();
+
+  // 1 Start
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_START, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 2 Stop
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 3 Busy
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 4 Tra
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 5 Master
+  if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+  LED1_OFF();
+
+//#define I2C_DEBUG_LED_CONTROL
+#ifdef I2C_DEBUG_LED_CONTROL
+
+
+  LED1_ON();
+
+  // 1 Anything CR?
+  if (( CR1) != 0x0000)
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 2 PE
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_PE, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  // 3 SWRESET
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_SWRST, CR1 ) )
+    LED2_ON();
+  else
+    LED2_OFF();
+  LED2_OFF();
+
+  LED1_OFF();
+#endif
+
+}
+#endif
+
+static inline void PPRZ_I2C_SEND_STOP(I2C_TypeDef *regs)
+{
+  // Man: p722:  Stop generation after the current byte transfer or after the current Start condition is sent.
+  regs->CR1 |= I2C_CR1_BIT_STOP;
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+}
+
+// (RE)START
+
+static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  periph->idx_buf = 0;
+
+#ifdef I2C_DEBUG_LED
+  LED_SHOW_ACTIVE_BITS(regs);
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+
+#endif
+
+/*
+  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
+  {
+    regs->CR1 &= ~ I2C_CR1_BIT_STOP;
+  }
+*/
+
+  // Enable Error IRQ, Event IRQ but disable Buffer IRQ
+  regs->CR2 |= I2C_CR2_BIT_ITERREN;
+  regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+  // Issue a new start
+  regs->CR1 =  (I2C_CR1_BIT_START | I2C_CR1_BIT_PE);
+  periph->status = I2CStartRequested;
+
+
+#ifdef I2C_DEBUG_LED
+  LED_SHOW_ACTIVE_BITS(regs);
+#endif
+}
+
+// STOP
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+//	SUBTRANSACTION SEQUENCES
+
+enum STMI2CSubTransactionStatus {
+  STMI2C_SubTra_Busy,
+  STMI2C_SubTra_Ready_StopRequested,
+  STMI2C_SubTra_Ready,
+  STMI2C_SubTra_Error
+};
+
+static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // Disable buffer interrupt
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // Send Slave address and wait for ADDR interrupt
+    regs->DR = trans->slave_addr;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // Now read SR2 to clear the ADDR
+    uint16_t SR2  __attribute__ ((unused)) = regs->SR2;
+
+    // Maybe check we are transmitting (did not loose arbitration for instance)
+    // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
+
+    // Send First max 2 bytes
+    regs->DR = trans->buf[0];
+    if (trans->len_w > 1)
+    {
+      regs->DR = trans->buf[1];
+      periph->idx_buf = 2;
+    }
+    else
+    {
+      periph->idx_buf = 1;
+    }
+
+    // Enable buffer-space available interrupt
+    // only if there is more to send: wait for TXE, no more to send: wait for BTF
+    if ( periph->idx_buf < trans->len_w)
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  // The buffer is not full anymore AND we were not waiting for BTF
+  else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
+  {
+    // Send the next byte
+    regs->DR = trans->buf[periph->idx_buf];
+    periph->idx_buf++;
+
+    // All bytes Sent? Then wait for BTF instead
+    if ( periph->idx_buf >= trans->len_w)
+    {
+      // Not interested anymore to know the buffer has space left
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+      // Next interrupt will be BTF (or error)
+    }
+  }
+  // BTF: means last byte was sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+      if (trans->type == I2CTransTx)
+      {
+        // Tell the driver we are ready
+        trans->status = I2CTransSuccess;
+      }
+
+      return STMI2C_SubTra_Ready;
+  }
+  else // Hardware error
+  {
+    return STMI2C_SubTra_Error;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // First Clear the ACK bit: after the next byte we do not want new bytes
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // --- next to steps MUST be executed together to avoid missing the stop
+    __I2C_REG_CRITICAL_ZONE_START;
+
+    // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+
+    // Schedule a Stop
+    PPRZ_I2C_SEND_STOP(regs);
+
+    __I2C_REG_CRITICAL_ZONE_STOP;
+    // --- end of critical zone -----------
+
+    // Enable the RXNE to get the result
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    trans->buf[0] = regs->DR;
+
+    // We got all the results (stop condition might still be in progress but this is the last interrupt)
+    trans->status = I2CTransSuccess;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+  else // Hardware error
+  {
+    return STMI2C_SubTra_Error;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    regs->CR1 |= I2C_CR1_BIT_POS;
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
+    // clearing ACK after the byte transfer has already started will NACK the next (2nd)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+
+    // --- make absolutely sure this command is not delayed too much after the previous:
+    __I2C_REG_CRITICAL_ZONE_START;
+    //       if transfer of DR was finished already then we will get too many bytes
+    // NOT First Clear the ACK bit but only AFTER clearing ADDR
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Disable the RXNE and wait for BTF
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+    __I2C_REG_CRITICAL_ZONE_STOP;
+    // --- end of critical zone -----------
+  }
+  // Receive buffer if full, master is halted: BTF
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Stop condition MUST be set BEFORE reading the DR
+    // otherwise since there is new buffer space a new byte will be read
+    PPRZ_I2C_SEND_STOP(regs);
+
+    trans->buf[0] = regs->DR;
+    trans->buf[1] = regs->DR;
+
+    // We got all the results
+    trans->status = I2CTransSuccess;
+
+    return STMI2C_SubTra_Ready_StopRequested;
+  }
+  else // Hardware error
+  {
+    return STMI2C_SubTra_Error;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Start Condition Was Just Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    // The first data byte will be acked in read many so the slave knows it should send more
+    regs->CR1 &= ~ I2C_CR1_BIT_POS;
+    regs->CR1 |= I2C_CR1_BIT_ACK;
+    // Clear the SB flag
+    regs->DR = trans->slave_addr | 0x01;
+  }
+  // Address Was Sent
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    periph->idx_buf = 0;
+
+    // Enable RXNE: receive an interrupt any time a byte is available
+    // only enable if MORE than 3 bytes need to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+
+    // ACK is still on to get more DATA
+    // Read SR2 to clear the ADDR (next byte will start arriving)
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // one or more bytes are available AND we were interested in Buffer interrupts
+  else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
+  {
+    // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+    }
+    // from : 3bytes -> last byte: do nothing
+    //
+    // finally: this was the last byte
+    else if (periph->idx_buf >= (trans->len_r - 1))
+    {
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+
+      // Last Value
+      trans->buf[periph->idx_buf] = regs->DR;
+      periph->idx_buf ++;
+
+      // We got all the results
+      trans->status = I2CTransSuccess;
+
+      return STMI2C_SubTra_Ready_StopRequested;
+    }
+
+    // Check for end of transaction: start waiting for BTF instead of RXNE
+    if (periph->idx_buf < (trans->len_r - 3))
+    {
+      regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+    }
+    else // idx >= len-3: there are 3 bytes to be read
+    {
+      // We want to halt I2C to have sufficient time to clear ACK, so:
+      // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer
+      // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted)
+      regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
+    }
+  }
+  // Buffer is full while this was not a RXNE interrupt
+  else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // Now the shift register and data register contain data(n-2) and data(n-1)
+    // And I2C is halted so we have time
+
+    // --- Make absolutely sure the next 2 I2C actions are performed with no delay
+    __I2C_REG_CRITICAL_ZONE_START;
+
+    // First we clear the ACK while the SCL is held low by BTF
+    regs->CR1 &= ~ I2C_CR1_BIT_ACK;
+
+    // Now that ACK is cleared we read one byte: instantly the last byte is being clocked in...
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+
+    // Now the last byte is being clocked. Stop in MUST be set BEFORE the transfer of the last byte is complete
+    PPRZ_I2C_SEND_STOP(regs);
+
+    __I2C_REG_CRITICAL_ZONE_STOP;
+    // --- end of critical zone -----------
+
+    // read the byte2 we had in the buffer (BTF means 2 bytes available)
+    trans->buf[periph->idx_buf] = regs->DR;
+    periph->idx_buf ++;
+
+    // Ask for an interrupt to read the last byte (which is normally still busy now)
+    // The last byte will be received with RXNE
+    regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+  }
+  else // Hardware error
+  {
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+#endif
+    return STMI2C_SubTra_Error;
+  }
+
+  return STMI2C_SubTra_Busy;
+}
+
+
+static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
+{
+  uint16_t SR1 = regs->SR1;
+
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
+
+  //regs->CR1 &= ~ I2C_CR1_BIT_PE;		// Disable Periferial
+  //regs->CR1 |=   I2C_CR1_BIT_PE;		// Enable Periferial
+
+  // Start Condition Was Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // SB: cleared by software when reading SR1 and writing to DR
+    regs->DR = 0x00;
+  }
+  // Address Was Sent
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // ADDR: Cleared by software when reading SR1 and then SR2
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // Byte Transfer Finished
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // SB: cleared by software when reading SR1 and reading/writing to DR
+    uint8_t dummy __attribute__ ((unused)) = regs->DR;
+    regs->DR = 0x00;
+  }
+
+
+  // Still have a start sheduled
+//  if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_START, regs->CR1) )
+  {
+    // Clear pending start conditions
+//    regs->CR1 &= ~ I2C_CR1_BIT_START;
+  }
+
+}
+
+
+static inline void i2c_error(struct i2c_periph *periph);
+
+static inline void i2c_irq(struct i2c_periph *periph)
+{
+
+  /*
+	There are 7 possible reasons to get here:
+
+	If IT_EV_FEN
+	-------------------------
+
+	We are always interested in all IT_EV_FEV: all are required.
+
+	1) SB		// Start Condition Success in Master mode
+	2) ADDR		// Address sent received Acknoledge
+	[3 ADDR10]	// -- 10bit address stuff
+	[4 STOPF]	// -- only for slaves: master has no stop interrupt
+	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
+
+	// Beware: using the buffered I2C has some interesting properties:
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
+           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
+	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
+	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
+           a new start in one go.
+
+          -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
+           This also means you must think more in advance and a transaction could be popped from the stack even before it is
+           actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
+           is known.
+
+	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
+
+	If IT_EV_FEN AND IT_EV_BUF
+	--------------------------
+
+	Buffer event are not always wanted and are tipically switched on during longer data transfers. Make sure to turn off in time.
+
+	6) RxNE
+	7) TxE
+
+	--------------------------------------------------------------------------------------------------
+	// This driver uses only a subset of the pprz_i2c_states for several reasons:
+	// -we have less interrupts than the I2CStatus states (for efficiency)
+	// -STM32 has such a powerfull I2C engine with plenty of status register flags that
+            only little extra status information needs to be stored.
+
+       // Status is re-used (abused) to remember the last COMMAND THAT WAS SENT to the STM I2C hardware.
+
+// TODO: check which are used
+	enum I2CStatus {
+	  I2CIdle,			// No more last command
+
+	  I2CStartRequested,		// Last command was start
+	  I2CRestartRequested,		// Last command was restart
+	  I2CStopRequested,		// Very important to not send double stop conditions
+
+	  I2CSendingByte,		// Some address/data operation
+
+	  // Following are not used
+	  I2CReadingByte,
+	  I2CAddrWrSent,
+	  I2CAddrRdSent,
+	  I2CSendingLastByte,
+	  I2CReadingLastByte,
+	  I2CComplete,
+	  I2CFailed
+	};
+
+	---------
+
+	The STM waits indefinately (holding SCL low) for user interaction:
+	a) after a master-start (waiting for address)
+	b) after an address (waiting for data)
+	   not during data sending when using buffered
+	c) after the last byte is transmitted (waiting for either stop or restart)
+	   not during data receiving when using buffered
+	   not after the last byte is received
+
+	-The STM I2C stalls indefinately when a stop condition was attempted that
+	did not succeed. The BUSY flag remains on.
+        -There is no STOP interrupt: use needs another way to finish.
+
+   */
+
+
+  ///////////////////////////////////////////////////////////////////////////////////
+  // Reading the status:
+  // - Caution: this clears several flags and can start transmissions etc...
+  // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
+  //   the transmission of the byte is finished. At higher clock rates that can be
+  //   quite fast: so we allow no other interrupt to be triggered in between
+  //   reading the status and setting all needed flags
+
+  // Direct Access to the I2C Registers
+  // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+#ifdef I2C_DEBUG_LED
+  LED1_ON();
+  LED1_OFF();
+#endif
+
+
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////////////////////////
+  //
+  //	TRANSACTION HANDLER
+
+  enum STMI2CSubTransactionStatus ret = 0;
+
+  ///////////////////////
+  // Nothing Left To Do
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+
+    // no transaction and also an error?
+    LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+    // If we still get an interrupt but there are no more things to do
+    // (which can happen if an event was sheduled just before a bus error occurs)
+    // then its easy: just stop: clear all interrupt generating bits
+
+    // Count The Errors
+    i2c_error(periph);
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
+    // Mark this as a special error
+    periph->errors->unexpected_event_cnt++;
+
+    periph->status = I2CIdle;
+
+    // There are no transactions anymore:
+    // furtheron we need a transaction pointer: so we are not allowed to continue
+    return;
+  }
+
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+
+  ///////////////////////////
+  // If there was an error:
+  if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+  {
+
+#ifdef I2C_DEBUG_LED
+        LED1_ON();
+        LED2_ON();
+	LED1_OFF();
+	LED2_OFF();
+
+        LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+    // Set result in transaction
+    trans->status = I2CTransFailed;
+
+    // Prepare for next
+    ret = STMI2C_SubTra_Ready;
+
+    // Make sure a TxRx does not Restart
+    trans->type = I2CTransRx;
+
+/*
+    // There are 2 types of errors: some need a STOP, some better do without: Following will not get an extra stop
+    if (
+           // Lost Arbitration
+           (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_ARLO, regs->SR1 ) )
+           // Buss Error When Master Only
+        || ((BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_BUS,  regs->SR1 ) )  &&  (!BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL,  regs->SR2 ) ))
+        || (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_OVR,  regs->SR1 ) )
+       )
+    {
+      ret = STMI2C_SubTra_Error;
+    }
+*/
+
+    // Count The Errors
+    i2c_error(periph);
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
+  }
+
+
+  ///////////////////////////
+  // Normal Event:
+  else
+  {
+
+    if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
+    {
+      switch (trans->len_r)
+      {
+        case 1:
+          ret = stmi2c_read1(regs,trans);
+          break;
+        case 2:
+          ret = stmi2c_read2(regs,trans);
+          break;
+        default:
+          ret = stmi2c_readmany(regs,periph, trans);
+          break;
+      }
+    }
+    else // TxRx or Tx
+    {
+      ret = stmi2c_send(regs,periph,trans);
+    }
+  }
+
+  /////////////////////////////////
+  // Sub-transaction has finished
+  if (ret != STMI2C_SubTra_Busy)
+  {
+    // If a restart is not needed
+    if (trans->type != I2CTransTxRx)
+    {
+      // Ready, no stop condition set yet
+      if (ret == STMI2C_SubTra_Ready)
+      {
+
+        // Program a stop
+        PPRZ_I2C_SEND_STOP(regs);
+
+        // Silent any BTF that would occur before STOP is executed
+        regs->DR = 0x00;
+      }
+
+      // In case of unexpected condition: e.g. not slave, no event
+      if (ret == STMI2C_SubTra_Error)
+      {
+
+        trans->status = I2CTransFailed;
+
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+#endif
+
+        LED_SHOW_ACTIVE_BITS(regs);
+
+        // Clear Running Events
+        stmi2c_clear_pending_interrupts(regs);
+
+      }
+
+
+      // Jump to the next transaction
+      periph->trans_extract_idx++;
+      if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+        periph->trans_extract_idx = 0;
+
+      // Tell everyone we are ready
+      periph->status = I2CIdle;
+
+
+      // if we have no more transaction to process, stop here
+      if (periph->trans_extract_idx == periph->trans_insert_idx)
+      {
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+      }
+      // if not, start next transaction
+      else
+      {
+        // Restart transaction doing the Rx part now
+// --- moved to idle function
+        PPRZ_I2C_SEND_START(periph);
+// ------
+     }
+
+    }
+    // RxTx -> Restart and do Rx part
+    else
+    {
+      trans->type = I2CTransRx;
+      periph->status = I2CStartRequested;
+      regs->CR1 |= I2C_CR1_BIT_START;
+
+      // Silent any BTF that would occur before SB
+      regs->DR = 0x00;
+    }
+  }
+
+  return;
+}
+
+static inline void i2c_error(struct i2c_periph *periph)
+{
+  uint8_t err_nr = 0;
+  periph->errors->er_irq_cnt;
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_AF)) {       /* Acknowledge failure */
+    periph->errors->ack_fail_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_AF);
+    err_nr = 1;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_BERR)) {     /* Misplaced Start or Stop condition */
+    periph->errors->miss_start_stop_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_BERR);
+    err_nr = 2;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_ARLO)) {     /* Arbitration lost */
+    periph->errors->arb_lost_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_ARLO);
+    err_nr = 3;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_OVR)) {      /* Overrun/Underrun */
+    periph->errors->over_under_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_OVR);
+    err_nr = 4;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_PECERR)) {   /* PEC Error in reception */
+    periph->errors->pec_recep_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_PECERR);
+    err_nr = 5;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_TIMEOUT)) {  /* Timeout or Tlow error */
+    periph->errors->timeout_tlow_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_TIMEOUT);
+    err_nr = 6;
+  }
+  if (I2C_GetITStatus(periph->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+    periph->errors->smbus_alert_cnt++;
+    I2C_ClearITPendingBit(periph->reg_addr, I2C_IT_SMBALERT);
+    err_nr = 7;
+  }
+
+#ifdef I2C_DEBUG_LED
+  LED_ERROR(20, err_nr);
+#endif
+
+  return;
+
+}
+
+
+/*
+  // Make sure the bus is free before resetting (p722)
+  if (regs->SR2 & (I2C_FLAG_BUSY >> 16)) {
+    // Reset the I2C block
+    I2C_SoftwareResetCmd(periph->reg_addr, ENABLE);
+    I2C_SoftwareResetCmd(periph->reg_addr, DISABLE);
+  }
+*/
+
+//#endif /* USE_I2C2 */
+
+
+
+
+#ifdef USE_I2C1
+
+struct i2c_errors i2c1_errors;
+
+void i2c1_hw_init(void) {
+
+  i2c1.reg_addr = I2C1;
+  i2c1.init_struct = &I2C1_InitStruct;
+  i2c1.scl_pin = GPIO_Pin_6;
+  i2c1.sda_pin = GPIO_Pin_7;
+  i2c1.errors = &i2c1_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c1_errors);
+
+  // Extra
+#ifdef I2C_DEBUG_LED
+  LED_INIT();
+#else
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C1);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C1 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C1 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C1 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c1.scl_pin | i2c1.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C1);
+
+  // enable peripheral
+  I2C_Cmd(I2C1, ENABLE);
+
+  I2C_Init(I2C1, i2c1.init_struct);
+
+  // enable error interrupts
+  I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE);
+
+#endif
+}
+
+void i2c1_ev_irq_handler(void) {
+  i2c_irq(&i2c1);
+}
+
+void i2c1_er_irq_handler(void) {
+  i2c_irq(&i2c1);
+}
+
+#endif /* USE_I2C1 */
+
+#ifdef USE_I2C2
+
+struct i2c_errors i2c2_errors;
+
+void i2c2_hw_init(void) {
+
+  i2c2.reg_addr = I2C2;
+  i2c2.init_struct = &I2C2_InitStruct;
+  i2c2.scl_pin = GPIO_Pin_10;
+  i2c2.sda_pin = GPIO_Pin_11;
+  i2c2.errors = &i2c2_errors;
+
+  /* zeros error counter */
+  ZEROS_ERR_COUNTER(i2c2_errors);
+
+  /* reset peripheral to default state ( sometimes not achieved on reset :(  ) */
+  //I2C_DeInit(I2C2);
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  NVIC_InitTypeDef  NVIC_InitStructure;
+
+  /* Configure and enable I2C2 event interrupt --------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Configure and enable I2C2 err interrupt ----------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Enable peripheral clocks -------------------------------------------------*/
+  /* Enable I2C2 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = i2c2.scl_pin | i2c2.sda_pin;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  I2C_DeInit(I2C2);
+
+  // enable peripheral
+  I2C_Cmd(I2C2, ENABLE);
+
+  I2C_Init(I2C2, i2c2.init_struct);
+
+  // enable error interrupts
+  I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
+
+}
+
+
+
+void i2c2_ev_irq_handler(void) {
+  i2c_irq(&i2c2);
+}
+
+void i2c2_er_irq_handler(void) {
+  i2c_irq(&i2c2);
+}
+
+#endif /* USE_I2C2 */
+
+
+void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
+{
+  if (i2c_idle(periph))
+  {
+    if (periph == &i2c2)
+    {
+      I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+      I2C_Init(I2C2, i2c2.init_struct);
+    }
+
+#ifdef I2C_DEBUG_LED
+        __disable_irq();
+
+        LED2_ON();
+        LED1_ON();
+        LED2_OFF();
+        LED1_OFF();
+        LED2_ON();
+        LED1_ON();
+        LED2_OFF();
+        LED1_OFF();
+
+        __enable_irq();
+#endif
+
+   }
+}
+
+void i2c_event(void)
+{
+  static uint32_t cnt = 0;
+  //I2C_TypeDef *regs;
+  cnt++;
+  if (cnt > 10000) cnt = 0;
+
+#ifndef I2C_DEBUG_LED
+#ifdef USE_I2C1
+  if (i2c1.status == I2CIdle)
+  {
+    if (i2c_idle(&i2c1))
+    {
+      __disable_irq();
+      // More work to do
+      if (i2c1.trans_extract_idx != i2c1.trans_insert_idx)
+      {
+        // Restart transaction doing the Rx part now
+        PPRZ_I2C_SEND_START(&i2c1);
+      }
+      __enable_irq();
+    }
+  }
+#endif
+#endif
+
+#ifdef USE_I2C2
+
+#ifdef I2C_DEBUG_LED
+  if (cnt == 0)
+  {
+        __disable_irq();
+
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+        if (i2c2.status == I2CIdle)
+        {
+          LED1_ON();
+   	  LED1_OFF();
+        }
+        else if (i2c2.status == I2CStartRequested)
+        {
+          LED1_ON();
+   	  LED1_OFF();
+          LED1_ON();
+   	  LED1_OFF();
+
+        }
+	LED2_OFF();
+
+        //regs = (I2C_TypeDef *) i2c2.reg_addr;
+        //LED_SHOW_ACTIVE_BITS(regs);
+
+        __enable_irq();
+  }
+#endif
+
+
+  //if (i2c2.status == I2CIdle)
+  {
+    //if (i2c_idle(&i2c2))
+    {
+      //__disable_irq();
+      // More work to do
+      //if (i2c2.trans_extract_idx != i2c2.trans_insert_idx)
+      {
+        // Restart transaction doing the Rx part now
+        //PPRZ_I2C_SEND_START(&i2c2);
+      }
+      //__enable_irq();
+    }
+  }
+#endif
+}
+
+/////////////////////////////////////////////////////////
+// Implement Interface Functions
+
+bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
+
+  uint8_t temp;
+  temp = periph->trans_insert_idx + 1;
+  if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
+  if (temp == periph->trans_extract_idx)
+    return FALSE;                          // queue full
+
+  t->status = I2CTransPending;
+
+  __disable_irq();
+  /* put transacation in queue */
+  periph->trans[periph->trans_insert_idx] = t;
+  periph->trans_insert_idx = temp;
+
+  /* if peripheral is idle, start the transaction */
+  // if (PPRZ_I2C_IS_IDLE(p))
+  if (periph->status == I2CIdle)
+  {
+    //if (i2c_idle(periph))
+    {
+#ifdef I2C_DEBUG_LED
+	if (periph == &i2c1)
+	{
+
+        }
+        else
+#endif
+        {
+          PPRZ_I2C_SEND_START(periph);
+        }
+    }
+  }
+  /* else it will be started by the interrupt handler when the previous transactions completes */
+  __enable_irq();
+
+  return TRUE;
+}
+
+bool_t i2c_idle(struct i2c_periph* periph)
+{
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+#ifdef I2C_DEBUG_LED
+	if (periph == &i2c1)
+	{
+	  return TRUE;
+        }
+#endif
+  if (periph->status == I2CIdle)
+    return ! (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, regs->SR2 ) );
+  else
+    return FALSE;
+}
+
+
diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h
index 43f8082e42..b0df164c10 100644
--- a/sw/airborne/mcu_periph/i2c.h
+++ b/sw/airborne/mcu_periph/i2c.h
@@ -127,7 +127,6 @@ extern void i2c1_init(void);
 
 extern struct i2c_periph i2c2;
 extern void i2c2_init(void);
-extern void i2c2_setbitrate(int bitrate);
 
 #endif /* USE_I2C2 */
 

From 0118e49e20bf8e1224c28b9c4369a618e835923a Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 23 Nov 2011 22:01:05 +0100
Subject: [PATCH 61/74] Disable other interrupt while first is running

---
 conf/airframes/CDW/debug_i2c.xml               |  2 +-
 .../arch/stm32/mcu_periph/i2c_arch.rewritten.c | 18 ++++++++++++++++--
 sw/airborne/boards/lisa_l/baro_board.c         |  1 +
 sw/airborne/modules/benchmark/i2c_abuse_test.c | 17 ++++++-----------
 sw/airborne/modules/benchmark/i2c_abuse_test.h |  2 +-
 5 files changed, 25 insertions(+), 15 deletions(-)

diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
index 7dd9a616eb..27d5dfdf5b 100644
--- a/conf/airframes/CDW/debug_i2c.xml
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -220,7 +220,7 @@
      <define name="FAILSAFE_GROUND_DETECT"/>
      <define name="USE_GPS_ACC4R"/>
      <target name="ap" board="lisa_l_1.0">
-      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="radio_control" type="ppm"/>
       <subsystem name="actuators"     type="asctec"/>
     </target>
     <target name="sim" board="pc">
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 8fb1a4da48..a75d606e89 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1106,11 +1106,17 @@ void i2c1_hw_init(void) {
 }
 
 void i2c1_ev_irq_handler(void) {
+  I2C_TypeDef *regs = (I2C_TypeDef *) i2c1.reg_addr;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITERREN;
   i2c_irq(&i2c1);
+  regs->CR2 |= I2C_CR2_BIT_ITERREN;
 }
 
 void i2c1_er_irq_handler(void) {
+  I2C_TypeDef *regs = (I2C_TypeDef *) i2c1.reg_addr;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITEVTEN;
   i2c_irq(&i2c1);
+  regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
 }
 
 #endif /* USE_I2C1 */
@@ -1174,14 +1180,18 @@ void i2c2_hw_init(void) {
 
 }
 
-
-
 void i2c2_ev_irq_handler(void) {
+  I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITERREN;
   i2c_irq(&i2c2);
+  regs->CR2 |= I2C_CR2_BIT_ITERREN;
 }
 
 void i2c2_er_irq_handler(void) {
+  I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
+  regs->CR2 &= ~ I2C_CR2_BIT_ITEVTEN;
   i2c_irq(&i2c2);
+  regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
 }
 
 #endif /* USE_I2C2 */
@@ -1322,11 +1332,13 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
     //if (i2c_idle(periph))
     {
 #ifdef I2C_DEBUG_LED
+#ifdef USE_I2C1
 	if (periph == &i2c1)
 	{
 
         }
         else
+#endif
 #endif
         {
           PPRZ_I2C_SEND_START(periph);
@@ -1344,10 +1356,12 @@ bool_t i2c_idle(struct i2c_periph* periph)
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
 #ifdef I2C_DEBUG_LED
+#ifdef USE_I2C1
 	if (periph == &i2c1)
 	{
 	  return TRUE;
         }
+#endif
 #endif
   if (periph->status == I2CIdle)
     return ! (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, regs->SR2 ) );
diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c
index 5fc013b62b..e8b6f92be8 100644
--- a/sw/airborne/boards/lisa_l/baro_board.c
+++ b/sw/airborne/boards/lisa_l/baro_board.c
@@ -25,6 +25,7 @@ void baro_init(void) {
 
 
 void baro_periodic(void) {
+#warning BARO_FILE_CODE_DISABLED
   return;
 
   switch (baro_board.status) {
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 7a9b078caa..1dd302faa9 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *
- * Copyright (C) 2009  Gautier Hattenberger
+ * Copyright (C) 2011  Christophe De Wagter
  *
  * This file is part of paparazzi.
  *
@@ -169,16 +169,6 @@ static void i2c_abuse_send_transaction(uint8_t _init)
 
 void event_i2c_abuse_test(void)
 {
-  if (i2c_idle(&i2c1))
-  {
-    LED_ON(7);	// green = idle
-    LED_OFF(6);
-  }
-  else
-  {
-    LED_ON(6); // red = busy
-    LED_OFF(7);
-  }
 
   if (i2c_idle(&i2c2))
   {
@@ -223,7 +213,12 @@ void event_i2c_abuse_test(void)
 			      i2c_abuse_test_bitrate += 17000;
 			      if (i2c_abuse_test_bitrate > 500000)
 			      {
+                                static uint8_t bit = 0;
 				i2c_abuse_test_bitrate -= 500000;
+                                bit = 1 - bit;
+				if (bit==0)
+				{ LED_ON(7);} else { LED_OFF(7); }
+				LED_OFF(6);
 			      }
 		  	      LED_TOGGLE(4);
 			    }
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.h b/sw/airborne/modules/benchmark/i2c_abuse_test.h
index e6e7b0634a..bc58dc900d 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.h
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.h
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *
- * Copyright (C) 2009  C. De Wagter
+ * Copyright (C) 2011  Christophe De Wagter
  *
  * This file is part of paparazzi.
  *

From 9f52491f31569a10fd6262d7bcf7be5c4ca2e5b6 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 24 Nov 2011 13:10:19 +0100
Subject: [PATCH 62/74] :-) :-) :-) :-)

---
 conf/modules/i2c_abuse_test.xml                    |  3 +--
 .../arch/stm32/mcu_periph/i2c_arch.rewritten.c     | 13 ++++++++++++-
 sw/airborne/boards/lisa_l/baro_board.c             |  2 +-
 sw/airborne/modules/benchmark/i2c_abuse_test.c     | 14 +++++---------
 4 files changed, 19 insertions(+), 13 deletions(-)

diff --git a/conf/modules/i2c_abuse_test.xml b/conf/modules/i2c_abuse_test.xml
index c8c7039eb4..db2f189213 100644
--- a/conf/modules/i2c_abuse_test.xml
+++ b/conf/modules/i2c_abuse_test.xml
@@ -5,10 +5,9 @@
     <file name="i2c_abuse_test.h"/>
   </header>
   <init fun="init_i2c_abuse_test()"/>
+  <periodic fun="periodic_50Hz_i2c_abuse_test()" period="0.02" autorun="TRUE" /> 
   <event fun="event_i2c_abuse_test()" />
- <!-- <periodic fun="periodic_50Hz_i2c_abuse_test()" period="0.02" autorun="FALSE"/> -->
   <makefile>
-    <!-- define name="i2c_abuse_test_MODULE_LED" value="2"/ -->
     <file name="i2c_abuse_test.c"/>
   </makefile>
 </module>
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index a75d606e89..695be6bca2 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1203,8 +1203,15 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
   {
     if (periph == &i2c2)
     {
+//      I2C_DeInit(I2C1);
+//      I2C_Cmd(I2C2, DISABLE);
+
       I2C2_InitStruct.I2C_ClockSpeed = bitrate;
-      I2C_Init(I2C2, i2c2.init_struct);
+//      I2C_Cmd(I2C2, ENABLE);
+//      I2C_Init(I2C2, i2c2.init_struct);
+      // 1) Program peripheral input clock CR2: to get correct timings
+      // 2) Configure clock control registers
+      // 3) Configure rise time register
     }
 
 #ifdef I2C_DEBUG_LED
@@ -1341,6 +1348,10 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 #endif
 #endif
         {
+#ifdef I2C_DEBUG_LED
+	LED2_ON();
+	LED2_OFF();
+#endif
           PPRZ_I2C_SEND_START(periph);
         }
     }
diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c
index e8b6f92be8..bbfd84ff3c 100644
--- a/sw/airborne/boards/lisa_l/baro_board.c
+++ b/sw/airborne/boards/lisa_l/baro_board.c
@@ -25,7 +25,7 @@ void baro_init(void) {
 
 
 void baro_periodic(void) {
-#warning BARO_FILE_CODE_DISABLED
+#warning BARO_BOARD_CODE_DISABLED
   return;
 
   switch (baro_board.status) {
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 1dd302faa9..ea9420d4a3 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -169,6 +169,8 @@ static void i2c_abuse_send_transaction(uint8_t _init)
 
 void event_i2c_abuse_test(void)
 {
+  static uint8_t bit1 = 0;
+  static uint8_t bit2 = 0;
 
   if (i2c_idle(&i2c2))
   {
@@ -197,6 +199,7 @@ void event_i2c_abuse_test(void)
 
   if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
   {
+	RunOnceEvery(100,LED_TOGGLE(7));
 	    if (i2c_abuse_test_counter < 16)
 	    {
 	       i2c_abuse_test_counter++;
@@ -208,32 +211,25 @@ void event_i2c_abuse_test(void)
 		{
 			      i2c_abuse_test_counter = 1;
 
-			      i2c_setbitrate(&i2c2, i2c_abuse_test_bitrate);
+			      //i2c_setbitrate(&i2c2, i2c_abuse_test_bitrate);
 
 			      i2c_abuse_test_bitrate += 17000;
 			      if (i2c_abuse_test_bitrate > 500000)
 			      {
-                                static uint8_t bit = 0;
 				i2c_abuse_test_bitrate -= 500000;
-                                bit = 1 - bit;
-				if (bit==0)
-				{ LED_ON(7);} else { LED_OFF(7); }
-				LED_OFF(6);
+                                bit1 = 1 - bit1;
 			      }
-		  	      LED_TOGGLE(4);
 			    }
 		}
 
 	    if (i2c_abuse_test_counter < 16)
 	    {
 	      i2c_abuse_send_transaction( i2c_abuse_test_counter );
-  	      LED_TOGGLE(5);
 	    }
   }
 }
 
 void periodic_50Hz_i2c_abuse_test(void) {
-  // LED_TOGGLE(DEMO_MODULE_LED);
 }
 
 

From 4a4eb450e32a259d521060b5909967cba4557693 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 24 Nov 2011 15:39:16 +0100
Subject: [PATCH 63/74] all possible I2C bitrates with runtime changing
 possible

---
 conf/modules/i2c_abuse_test.xml               |  2 +-
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 58 +++++++++++++++++--
 .../modules/benchmark/i2c_abuse_test.c        |  6 +-
 3 files changed, 56 insertions(+), 10 deletions(-)

diff --git a/conf/modules/i2c_abuse_test.xml b/conf/modules/i2c_abuse_test.xml
index db2f189213..4134a8b404 100644
--- a/conf/modules/i2c_abuse_test.xml
+++ b/conf/modules/i2c_abuse_test.xml
@@ -5,7 +5,7 @@
     <file name="i2c_abuse_test.h"/>
   </header>
   <init fun="init_i2c_abuse_test()"/>
-  <periodic fun="periodic_50Hz_i2c_abuse_test()" period="0.02" autorun="TRUE" /> 
+  <periodic fun="periodic_50Hz_i2c_abuse_test()" period="0.02" autorun="TRUE" />
   <event fun="event_i2c_abuse_test()" />
   <makefile>
     <file name="i2c_abuse_test.c"/>
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 695be6bca2..736d1b50ca 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1203,15 +1203,65 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
   {
     if (periph == &i2c2)
     {
-//      I2C_DeInit(I2C1);
-//      I2C_Cmd(I2C2, DISABLE);
+      int devider;
+      int risetime;
 
+      I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
+
+      // store (just for fun)
       I2C2_InitStruct.I2C_ClockSpeed = bitrate;
-//      I2C_Cmd(I2C2, ENABLE);
-//      I2C_Init(I2C2, i2c2.init_struct);
+
+/*****************************************************
+	Bitrate:
+
+	-CR2 + CCR + TRISE registers
+	-only change when PE=0
+
+	e.g.
+
+	10kHz:  36MHz + Standard 0x708 + 0x25
+	70kHz:  36MHz + Standard 0x101 +
+	400kHz: 36MHz + Fast 0x1E      + 0xb
+
       // 1) Program peripheral input clock CR2: to get correct timings
       // 2) Configure clock control registers
       // 3) Configure rise time register
+******************************************************/
+
+      if (bitrate < 3000)
+        bitrate = 3000;
+
+      // 36MHz, fast scl: 2counts low 1 count high -> / 3:
+      devider = 12000000UL / bitrate;
+
+      // never allow faster than 600kbps
+      if (devider < 20)
+        devider = 20;
+
+      // no overflow either
+      if (devider >=4095)
+        devider = 4095;
+
+      risetime = 1000000 / (bitrate/1000) / 8 / 28;
+
+      if (risetime < 10)
+        risetime = 10;
+
+      if (risetime >=31)
+        risetime = 31;
+
+
+      regs->CR1 &= ~ I2C_CR1_BIT_PE;
+
+      // 1)
+      regs->CR2 = 0x0324;
+      // 2)
+      regs->CCR = 0x8000 + devider;
+      // 3)
+      regs->TRISE = risetime;
+
+      regs->CR1 |=   I2C_CR1_BIT_PE;
+
     }
 
 #ifdef I2C_DEBUG_LED
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index ea9420d4a3..db26d82bb6 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -169,9 +169,6 @@ static void i2c_abuse_send_transaction(uint8_t _init)
 
 void event_i2c_abuse_test(void)
 {
-  static uint8_t bit1 = 0;
-  static uint8_t bit2 = 0;
-
   if (i2c_idle(&i2c2))
   {
     LED_ON(5);	// green = idle
@@ -211,13 +208,12 @@ void event_i2c_abuse_test(void)
 		{
 			      i2c_abuse_test_counter = 1;
 
-			      //i2c_setbitrate(&i2c2, i2c_abuse_test_bitrate);
+			      i2c_setbitrate(&i2c2, i2c_abuse_test_bitrate);
 
 			      i2c_abuse_test_bitrate += 17000;
 			      if (i2c_abuse_test_bitrate > 500000)
 			      {
 				i2c_abuse_test_bitrate -= 500000;
-                                bit1 = 1 - bit1;
 			      }
 			    }
 		}

From 0d0605c73398195c9384f8ab8242dd58e1ce4c64 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 24 Nov 2011 16:40:36 +0100
Subject: [PATCH 64/74] from 3kbps to 600kpbs

---
 .../arch/stm32/mcu_periph/i2c_arch.rewritten.c      | 13 +++++++------
 sw/airborne/modules/benchmark/i2c_abuse_test.c      |  4 ++--
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 736d1b50ca..473ff56b2a 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -5,7 +5,7 @@
 #include <stm32/flash.h>
 #include <stm32/misc.h>
 
-#define I2C_DEBUG_LED
+//#define I2C_DEBUG_LED
 
 /////////// DEBUGGING //////////////
 // TODO: remove this
@@ -920,9 +920,9 @@ static inline void i2c_irq(struct i2c_periph *periph)
         LED1_ON();
 	LED2_OFF();
 	LED1_OFF();
-#endif
 
         LED_SHOW_ACTIVE_BITS(regs);
+#endif
 
         // Clear Running Events
         stmi2c_clear_pending_interrupts(regs);
@@ -1203,8 +1203,8 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
   {
     if (periph == &i2c2)
     {
-      int devider;
-      int risetime;
+      volatile int devider;
+      volatile int risetime;
 
       I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
 
@@ -1232,7 +1232,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
         bitrate = 3000;
 
       // 36MHz, fast scl: 2counts low 1 count high -> / 3:
-      devider = 12000000UL / bitrate;
+      devider = 12000 / (bitrate/1000);
 
       // never allow faster than 600kbps
       if (devider < 20)
@@ -1242,7 +1242,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
       if (devider >=4095)
         devider = 4095;
 
-      risetime = 1000000 / (bitrate/1000) / 8 / 28;
+      risetime = 1000000 / (bitrate/1000) / 6 / 28;
 
       if (risetime < 10)
         risetime = 10;
@@ -1257,6 +1257,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
       regs->CR2 = 0x0324;
       // 2)
       regs->CCR = 0x8000 + devider;
+      //regs->CCR = 0x0000 + devider;
       // 3)
       regs->TRISE = risetime;
 
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index db26d82bb6..8ab843bd4d 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -29,8 +29,8 @@
 struct i2c_transaction i2c_test1;
 struct i2c_transaction i2c_test2;
 
-uint8_t i2c_abuse_test_counter = 0;
-uint16_t i2c_abuse_test_bitrate = 1000;
+volatile uint8_t i2c_abuse_test_counter = 0;
+volatile uint32_t i2c_abuse_test_bitrate = 1000;
 
 void init_i2c_abuse_test(void) {
   //LED_INIT(DEMO_MODULE_LED);

From 0bbc30a4c704a510c187fd511fcc3e7ae15984a9 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 24 Nov 2011 16:56:08 +0100
Subject: [PATCH 65/74] Make it a square wave please

---
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 473ff56b2a..33c6c69ea4 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -1232,7 +1232,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
         bitrate = 3000;
 
       // 36MHz, fast scl: 2counts low 1 count high -> / 3:
-      devider = 12000 / (bitrate/1000);
+      devider = 18000 / (bitrate/1000);
 
       // never allow faster than 600kbps
       if (devider < 20)
@@ -1256,8 +1256,8 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
       // 1)
       regs->CR2 = 0x0324;
       // 2)
-      regs->CCR = 0x8000 + devider;
-      //regs->CCR = 0x0000 + devider;
+      //regs->CCR = 0x8000 + devider;
+      regs->CCR = 0x0000 + devider;
       // 3)
       regs->TRISE = risetime;
 

From 3eae92fa9e2f5b626e07114d3dc0d333a89fcb3f Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 25 Nov 2011 14:18:46 +0100
Subject: [PATCH 66/74] Square I2C signal

---
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index 33c6c69ea4..afffc4ce21 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -81,7 +81,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
 //      .I2C_ClockSpeed = 37000
-      .I2C_ClockSpeed = 400000
+      .I2C_ClockSpeed = 300000
 };
 #endif
 
@@ -1102,6 +1102,7 @@ void i2c1_hw_init(void) {
   // enable error interrupts
   I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE);
 
+  i2c_setbitrate(&i2c2, I2C2_InitStruct.I2C_ClockSpeed);
 #endif
 }
 
@@ -1178,6 +1179,7 @@ void i2c2_hw_init(void) {
   // enable error interrupts
   I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
 
+  i2c_setbitrate(&i2c2, I2C2_InitStruct.I2C_ClockSpeed);
 }
 
 void i2c2_ev_irq_handler(void) {

From fcf199be5e8f1b363ceb31ba52988d847f766d98 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 25 Nov 2011 14:59:11 +0100
Subject: [PATCH 67/74] Change I2C bitrate on LPC21

---
 sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c | 39 +++++++++++++++++++-
 1 file changed, 38 insertions(+), 1 deletion(-)

diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index 49c46ed1ab..eb990fdebe 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -342,6 +342,43 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
 }
 
 void i2c_event(void) { }
-void i2c2_setbitrate(int bitrate __attribute__ ((unused))) { }
+void i2c_setbitrate(struct i2c_periph* p, int bitrate)
+{
+  int period = 15000000 / 2 / bitrate;
+  // Max 400kpbs
+  if (period < 19)
+    period = 19;
+  // Min 5kbps
+  if (period > 1500)
+    period = 1500;
+
+/* default clock speed 37.5KHz with our 15MHz PCLK
+   I2C1_CLOCK = PCLK / (I2C1_SCLL + I2C1_SCLH)     */
+
+#if (PCLK == 30000000)
+  period *= 2;
+#endif
+
+#if (PCLK == 60000000)
+  period *= 4;
+#endif
+
+#ifdef USE_I2C0
+  if (p == &i2c0)
+  {
+    /* set bitrate */
+    I2C0SCLL = period;
+    I2C0SCLH = period;
+  }
+#endif
+#ifdef USE_I2C1
+  if (p == &i2c1)
+  {
+    /* set bitrate */
+    I2C1SCLL = period;
+    I2C1SCLH = period;
+  }
+#endif
+}
 
 

From 96d0b6a233ea968e71767038d8cd5aa8e960a8a3 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 25 Nov 2011 14:59:53 +0100
Subject: [PATCH 68/74] Run I2C test on LPC

---
 conf/modules/i2c_abuse_test.xml               | 11 +++++
 .../modules/benchmark/i2c_abuse_test.c        | 46 +++++++++----------
 2 files changed, 34 insertions(+), 23 deletions(-)

diff --git a/conf/modules/i2c_abuse_test.xml b/conf/modules/i2c_abuse_test.xml
index 4134a8b404..50ddbd84c5 100644
--- a/conf/modules/i2c_abuse_test.xml
+++ b/conf/modules/i2c_abuse_test.xml
@@ -9,6 +9,17 @@
   <event fun="event_i2c_abuse_test()" />
   <makefile>
     <file name="i2c_abuse_test.c"/>
+    <raw>
+ifeq ($(ARCH), lpc21)
+$(TARGET).CFLAGS += -DI2C_ABUSE_LED=3
+$(TARGET).CFLAGS += -DUSE_I2C0
+$(TARGET).CFLAGS += -DI2C_ABUSE_PORT=i2c0
+else ifeq ($(ARCH), stm32)
+$(TARGET).CFLAGS += -DI2C_ABUSE_LED=7
+$(TARGET).CFLAGS += -DI2C_ABUSE_PORT=i2c2
+endif
+
+    </raw>
   </makefile>
 </module>
 
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 8ab843bd4d..62acfda617 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -62,59 +62,59 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.buf[2] = 0x01<<5;
     i2c_test1.buf[3] = 0x00;
     i2c_test1.len_w = 4;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
     break;
   case 2:
     i2c_test1.type = I2CTransTx;
     i2c_test1.buf[0] = 0x01;  // set to gain to 1 Gauss
     i2c_test1.buf[1] = 0x01<<5;
     i2c_test1.len_w = 2;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 3:
     i2c_test1.type = I2CTransTx;
     i2c_test1.buf[0] = 0x00;  // set to continuous mode
     i2c_test1.len_w = 1;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 4:
     i2c_test1.type = I2CTransRx;
     i2c_test1.len_r = 1;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 5:
     i2c_test1.type = I2CTransRx;
     i2c_test1.len_r = 2;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 6:
     i2c_test1.type = I2CTransRx;
     i2c_test1.len_r = 3;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 7:
     i2c_test1.type = I2CTransRx;
     i2c_test1.len_r = 4;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 8:
     i2c_test1.type = I2CTransRx;
     i2c_test1.len_r = 5;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 9:
     // bad addr
     i2c_test1.slave_addr = 0x3C + 2;
     i2c_test1.type = I2CTransTx;
     i2c_test1.len_w = 1;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 10:
     // 2 consecutive
     i2c_test1.type = I2CTransTx;
     i2c_test1.buf[0] = 0x00;  // set to continuous mode
     i2c_test1.len_w = 1;
-    i2c_submit(&i2c2,&i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT,&i2c_test1);
   break;
   case 11:
     i2c_test1.slave_addr = 0x3C;
@@ -122,7 +122,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.len_r = 1;
     i2c_test1.len_w = 1;
     i2c_test1.buf[0] = 0x03;
-    i2c_submit(&i2c2, &i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT, &i2c_test1);
   break;
   case 12:
     i2c_test1.slave_addr = 0x3C;
@@ -130,7 +130,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.len_r = 2;
     i2c_test1.len_w = 1;
     i2c_test1.buf[0] = 0x03;
-    i2c_submit(&i2c2, &i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT, &i2c_test1);
   break;
   case 13:
     i2c_test1.slave_addr = 0x3C;
@@ -138,7 +138,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.len_r = 3;
     i2c_test1.len_w = 1;
     i2c_test1.buf[0] = 0x03;
-    i2c_submit(&i2c2, &i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT, &i2c_test1);
   break;
   case 14:
     i2c_test1.slave_addr = 0x3C;
@@ -146,7 +146,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.len_r = 4;
     i2c_test1.len_w = 1;
     i2c_test1.buf[0] = 0x03;
-    i2c_submit(&i2c2, &i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT, &i2c_test1);
   break;
   case 15:
     i2c_test1.slave_addr = 0x3C;
@@ -154,7 +154,7 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.len_r = 4;
     i2c_test1.len_w = 2;
     i2c_test1.buf[0] = 0x03;
-    i2c_submit(&i2c2, &i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT, &i2c_test1);
   break;
   default:
     i2c_test1.slave_addr = 0x3C;
@@ -162,14 +162,14 @@ static void i2c_abuse_send_transaction(uint8_t _init)
     i2c_test1.len_r = 5;
     i2c_test1.len_w = 1;
     i2c_test1.buf[0] = 0x03;
-    i2c_submit(&i2c2, &i2c_test1);
+    i2c_submit(&I2C_ABUSE_PORT, &i2c_test1);
   }
 }
 
 
 void event_i2c_abuse_test(void)
 {
-  if (i2c_idle(&i2c2))
+  if (i2c_idle(&I2C_ABUSE_PORT))
   {
     LED_ON(5);	// green = idle
     LED_OFF(4);
@@ -189,14 +189,13 @@ void event_i2c_abuse_test(void)
 		  i2c_test2.type = I2CTransRx;
 		  i2c_test2.slave_addr = 0x92;
 		  i2c_test2.len_r = 2;
-	    i2c_submit(&i2c2,&i2c_test2);
+	    i2c_submit(&I2C_ABUSE_PORT,&i2c_test2);
 	  }
   }
 
 
   if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
   {
-	RunOnceEvery(100,LED_TOGGLE(7));
 	    if (i2c_abuse_test_counter < 16)
 	    {
 	       i2c_abuse_test_counter++;
@@ -204,22 +203,23 @@ void event_i2c_abuse_test(void)
 	    else
 	    {
 		// wait until ready:
-		if (i2c_idle(&i2c2))
+		if (i2c_idle(&I2C_ABUSE_PORT))
 		{
 			      i2c_abuse_test_counter = 1;
 
-			      i2c_setbitrate(&i2c2, i2c_abuse_test_bitrate);
+			      //i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate);
 
 			      i2c_abuse_test_bitrate += 17000;
-			      if (i2c_abuse_test_bitrate > 500000)
+			      if (i2c_abuse_test_bitrate > 410000)
 			      {
-				i2c_abuse_test_bitrate -= 500000;
+				i2c_abuse_test_bitrate -= 410000;
 			      }
 			    }
 		}
 
 	    if (i2c_abuse_test_counter < 16)
 	    {
+	      RunOnceEvery(100,LED_TOGGLE(I2C_ABUSE_LED));
 	      i2c_abuse_send_transaction( i2c_abuse_test_counter );
 	    }
   }

From eb58f6bf7a0d7d9ad82a4d6d6e68f534f60075fc Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 25 Nov 2011 15:06:22 +0100
Subject: [PATCH 69/74] Telemetry of errors sounds interesting for the next few
 months, preferable even default so we have a higher chance of getting error
 logs

---
 conf/telemetry/telemetry_booz2.xml           |  1 +
 sw/airborne/arch/stm32/mcu_periph/i2c_arch.h |  4 --
 sw/airborne/firmwares/rotorcraft/telemetry.h | 49 ++++++++++++++++----
 3 files changed, 40 insertions(+), 14 deletions(-)

diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index 36de95539b..f3fb74b27e 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -16,6 +16,7 @@
       <message name="BOOZ2_CAM"         period="1."/>
       <message name="GPS_INT"         period=".25"/>
       <message name="INS"          period=".25"/>
+      <message name="I2C_ERRORS"    period="6."/>
     </mode>
 
     <mode name="ppm">
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h
index 0274829fc1..50519ea24c 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.h
@@ -34,8 +34,6 @@
 
 #ifdef USE_I2C1
 
-extern struct i2c_errors i2c1_errors;
-
 extern void i2c1_hw_init(void);
 extern void i2c1_ev_irq_handler(void);
 extern void i2c1_er_irq_handler(void);
@@ -46,8 +44,6 @@ extern void i2c1_er_irq_handler(void);
 
 #ifdef USE_I2C2
 
-extern struct i2c_errors i2c2_errors;
-
 extern void i2c2_hw_init(void);
 extern void i2c2_ev_irq_handler(void);
 extern void i2c2_er_irq_handler(void);
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 59cc90d163..314adf80d5 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -52,8 +52,8 @@
 #endif
 #include "subsystems/ins.h"
 #include "subsystems/ahrs.h"
-//FIXME: wtf ??!!
-#include "mcu_periph/i2c_arch.h"
+// I2C Error counters
+#include "mcu_periph/i2c.h"
 
 extern uint8_t telemetry_mode_Main_DefaultChannel;
 
@@ -748,17 +748,46 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				   );					       \
   }
 
-#define PERIODIC_SEND_I2C_ERRORS(_chan) {				       \
+#ifdef USE_I2C1
+#define PERIODIC_SEND_I2C1_ERRORS(_chan) {				       \
     DOWNLINK_SEND_I2C_ERRORS(_chan,				       \
-				   &i2c_errc_ack_fail,  \
-				   &i2c_errc_miss_start_stop,  \
-				   &i2c_errc_arb_lost,  \
-				   &i2c_errc_over_under,  \
-				   &i2c_errc_pec_recep,  \
-				   &i2c_errc_timeout_tlow,  \
-				   &i2c_errc_smbus_alert  \
+				   &i2c1.errors->ack_fail_cnt,  \
+				   &i2c1.errors->miss_start_stop_cnt,  \
+				   &i2c1.errors->arb_lost_cnt,  \
+				   &i2c1.errors->over_under_cnt,  \
+				   &i2c1.errors->pec_recep_cnt,  \
+				   &i2c1.errors->timeout_tlow_cnt,  \
+				   &i2c1.errors->smbus_alert_cnt,  \
+				   &i2c1.errors->unexpected_event_cnt,  \
+				   &i2c1.errors->last_unexpected_event  \
 				   );					       \
   }
+#else
+#define PERIODIC_SEND_I2C2_ERRORS(_chan) {}
+#endif
+
+#ifdef USE_I2C2
+#define PERIODIC_SEND_I2C2_ERRORS(_chan) {				       \
+    DOWNLINK_SEND_I2C_ERRORS(_chan,				       \
+				   &i2c2.errors->ack_fail_cnt,  \
+				   &i2c2.errors->miss_start_stop_cnt,  \
+				   &i2c2.errors->arb_lost_cnt,  \
+				   &i2c2.errors->over_under_cnt,  \
+				   &i2c2.errors->pec_recep_cnt,  \
+				   &i2c2.errors->timeout_tlow_cnt,  \
+				   &i2c2.errors->smbus_alert_cnt,  \
+				   &i2c2.errors->unexpected_event_cnt,  \
+				   &i2c2.errors->last_unexpected_event  \
+				   );					       \
+  }
+#else
+#define PERIODIC_SEND_I2C2_ERRORS(_chan) {}
+#endif
+
+#define PERIODIC_SEND_I2C_ERRORS(_chan) {				       \
+    PERIODIC_SEND_I2C1_ERRORS(_chan);  						\
+    PERIODIC_SEND_I2C2_ERRORS(_chan);  						\
+}
 
 //TODO replace by BOOZ_EXTRA_ADC
 #ifdef BOOZ2_SONAR

From df325af3bf822e7f60ff7a9add636f92ced4dc59 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 25 Nov 2011 15:08:05 +0100
Subject: [PATCH 70/74] I2C test airframe on LPC

---
 conf/airframes/CDW/debug_i2c_lpc.xml | 215 +++++++++++++++++++++++++++
 1 file changed, 215 insertions(+)
 create mode 100644 conf/airframes/CDW/debug_i2c_lpc.xml

diff --git a/conf/airframes/CDW/debug_i2c_lpc.xml b/conf/airframes/CDW/debug_i2c_lpc.xml
new file mode 100644
index 0000000000..18320f15cc
--- /dev/null
+++ b/conf/airframes/CDW/debug_i2c_lpc.xml
@@ -0,0 +1,215 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="2" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="6" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@YAW"/>
+  </rc_commands>
+
+  <section name="SERVO_MIXER_GAINS">
+    <define name="AILERON_RATE_UP" value="0.50f"/>
+    <define name="AILERON_RATE_DOWN" value="0.25f"/>
+
+    <define name="AILERON_RATE_UP_BRAKE" value="0.5f"/>
+    <define name="AILERON_RATE_DOWN_BRAKE" value="0.9f"/>
+
+    <define name="PITCH_GAIN" value="0.9f"/>
+
+    <define name="YAW_THRUST" value="0.0f"/>
+    <define name="BRAKE_AILEVON" value="-0.68f"/>
+    <define name="BRAKE_PITCH" value="0.0f"/>
+    <define name="MAX_BRAKE_RATE" value="150"/>
+
+    <define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE)  )); \
+       int16_t _rate_limited_value = 0;"/>
+
+</section>
+
+  <command_laws>
+    <!-- Brake Rate Limiter -->
+    <let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
+    <!--<let var="brake_value" value="RATELIMIT(  $brake_value , MAX_BRAKE_RATE )"/>
+
+    <let var="test; \
+  	      static int16_t _var_brake_value = 0; \
+              _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
+              int verwaarloos_deze_warning_CDW" value="0"/>
+		-->
+
+    <!-- Differential Aileron Depending on Brake Value -->
+    <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
+    <let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
+    <let var="aileron_up" value="(@ROLL  *  (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="aileron_down" value="(@ROLL  * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
+    <let var="rightturn" value="(1 - $leftturn)"/>
+    <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON)"/>
+    <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON)"/>
+
+    <!-- Differential Thurst -->
+    <set servo="THROTTLE" value="@THROTTLE"/>
+
+    <!-- Pitch with Brake-Trim Function -->
+    <set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
+  </command_laws>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="0."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+    <load name="ins_xsens_MTiG_fixedwing.xml">
+      <configure name="XSENS_UART_NR" value="0"/>
+    </load>
+
+   <load name="i2c_abuse_test.xml"/>
+
+  </modules>
+
+  <firmware name="fixedwing">
+
+    <target name="ap" board="tiny_2.11">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+      <define name="LOITER_TRIM"/>
+      <define name="ALT_KALMAN"/>
+    </target>
+    <target name="sim" board="pc"/>
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B57600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+    <subsystem name="gps" type="xsens"/>
+
+
+</firmware>
+
+</airframe>

From 0cac27cb02502bd52622537ece306cf94695248d Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 25 Nov 2011 17:35:32 +0100
Subject: [PATCH 71/74] Commenting + Structuring: no logical changes

---
 .../stm32/mcu_periph/i2c_arch.rewritten.c     | 898 +++++++++---------
 .../modules/benchmark/i2c_abuse_test.c        |   2 +-
 2 files changed, 458 insertions(+), 442 deletions(-)

diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
index afffc4ce21..09f19ff27d 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.rewritten.c
@@ -80,7 +80,7 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
       .I2C_OwnAddress1 = 0x00,
       .I2C_Ack = I2C_Ack_Enable,
       .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-//      .I2C_ClockSpeed = 37000
+//      .I2C_ClockSpeed = 37500	// Like on LPC
       .I2C_ClockSpeed = 300000
 };
 #endif
@@ -137,8 +137,8 @@ static I2C_InitTypeDef  I2C2_InitStruct = {
 
 // Critical Zones
 
-#define __I2C_REG_CRITICAL_ZONE_START
-#define __I2C_REG_CRITICAL_ZONE_STOP
+#define __I2C_REG_CRITICAL_ZONE_START	__disable_irq();
+#define __I2C_REG_CRITICAL_ZONE_STOP	__enable_irq();
 
 
 //////////////////////////////////////////////////////////////////////////////////
@@ -284,6 +284,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 {
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
+  // Reset the buffer pointer to the first byte
   periph->idx_buf = 0;
 
 #ifdef I2C_DEBUG_LED
@@ -300,13 +301,6 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 
 #endif
 
-/*
-  if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, regs->CR1 ) )
-  {
-    regs->CR1 &= ~ I2C_CR1_BIT_STOP;
-  }
-*/
-
   // Enable Error IRQ, Event IRQ but disable Buffer IRQ
   regs->CR2 |= I2C_CR2_BIT_ITERREN;
   regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
@@ -316,10 +310,6 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
   regs->CR1 =  (I2C_CR1_BIT_START | I2C_CR1_BIT_PE);
   periph->status = I2CStartRequested;
 
-
-#ifdef I2C_DEBUG_LED
-  LED_SHOW_ACTIVE_BITS(regs);
-#endif
 }
 
 // STOP
@@ -328,6 +318,7 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
 ///////////////////////////////////////////////////////////////////////////////////////////////////////
 //
 //	SUBTRANSACTION SEQUENCES
+//	-We arrive here every time a ISR is called with no error
 
 enum STMI2CSubTransactionStatus {
   STMI2C_SubTra_Busy,
@@ -336,6 +327,8 @@ enum STMI2CSubTransactionStatus {
   STMI2C_SubTra_Error
 };
 
+// Doc ID 13902 Rev 11 p 710/1072
+// Transfer Sequence Diagram for Master Transmitter
 static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
   uint16_t SR1 = regs->SR1;
@@ -347,15 +340,18 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, str
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     // Send Slave address and wait for ADDR interrupt
     regs->DR = trans->slave_addr;
+    // Document the current Status
+    periph->status = I2CAddrWrSent;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
-    // Now read SR2 to clear the ADDR
+    // Now read SR2 to clear the ADDR status Bit
     uint16_t SR2  __attribute__ ((unused)) = regs->SR2;
 
     // Maybe check we are transmitting (did not loose arbitration for instance)
     // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { }
+    // update: this should be caught by the ARLO error: so we will not arrive here
 
     // Send First max 2 bytes
     regs->DR = trans->buf[0];
@@ -373,6 +369,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, str
     // only if there is more to send: wait for TXE, no more to send: wait for BTF
     if ( periph->idx_buf < trans->len_w)
       regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+
+    // Document the current Status
+    periph->status = I2CSendingByte;
   }
   // The buffer is not full anymore AND we were not waiting for BTF
   else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_TXE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
@@ -397,10 +396,11 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, str
         // Tell the driver we are ready
         trans->status = I2CTransSuccess;
       }
+      // Otherwise we still need to do the receiving part
 
       return STMI2C_SubTra_Ready;
   }
-  else // Hardware error
+  else // Event Logic Error
   {
     return STMI2C_SubTra_Error;
   }
@@ -408,7 +408,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(I2C_TypeDef *regs, str
   return STMI2C_SubTra_Busy;
 }
 
-static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, struct i2c_transaction *trans)
+// Doc ID 13902 Rev 11 p 714/1072
+// Transfer Sequence Diagram for Master Receiver for N=1
+static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
   uint16_t SR1 = regs->SR1;
 
@@ -417,6 +419,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     regs->DR = trans->slave_addr | 0x01;
+
+    // Document the current Status
+    periph->status = I2CAddrRdSent;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
@@ -437,9 +442,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     __I2C_REG_CRITICAL_ZONE_STOP;
     // --- end of critical zone -----------
 
-    // Enable the RXNE to get the result
+    // Enable the RXNE: it will trigger as soon as the 1 byte is received to get the result
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
+
+    // Document the current Status
+    periph->status = I2CReadingLastByte;
   }
+  // As soon as there is 1 byte ready to read, we have our byte
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) )
   {
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
@@ -448,9 +457,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
     // We got all the results (stop condition might still be in progress but this is the last interrupt)
     trans->status = I2CTransSuccess;
 
+    // Document the current Status: 
+    // -the stop was actually already requested in the previous step
+    periph->status = I2CStopRequested;
+
     return STMI2C_SubTra_Ready_StopRequested;
   }
-  else // Hardware error
+  else // Event Logic Error
   {
     return STMI2C_SubTra_Error;
   }
@@ -458,28 +471,36 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(I2C_TypeDef *regs, st
   return STMI2C_SubTra_Busy;
 }
 
-static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, struct i2c_transaction *trans)
+// Doc ID 13902 Rev 11 p 713/1072
+// Transfer Sequence Diagram for Master Receiver for N=2
+static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
   uint16_t SR1 = regs->SR1;
 
   // Start Condition Was Just Generated
   if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
   {
+    // according to the datasheet: instantly shedule a NAK on the second received byte:
     regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;
     regs->CR1 |= I2C_CR1_BIT_ACK;
     regs->CR1 |= I2C_CR1_BIT_POS;
     regs->DR = trans->slave_addr | 0x01;
+
+    // Document the current Status
+    periph->status = I2CAddrRdSent;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
   {
+    // --- make absolutely sure this command is not delayed too much after the previous:
+    // --- the NAK bits must be set before the first byte arrived: allow other interrupts here
+    __I2C_REG_CRITICAL_ZONE_START;
+
+    //       if transfer of DR was finished already then we will get too many bytes
     // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving)
     // clearing ACK after the byte transfer has already started will NACK the next (2nd)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
 
-    // --- make absolutely sure this command is not delayed too much after the previous:
-    __I2C_REG_CRITICAL_ZONE_START;
-    //       if transfer of DR was finished already then we will get too many bytes
     // NOT First Clear the ACK bit but only AFTER clearing ADDR
     regs->CR1 &= ~ I2C_CR1_BIT_ACK;
 
@@ -488,6 +509,11 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
 
     __I2C_REG_CRITICAL_ZONE_STOP;
     // --- end of critical zone -----------
+
+    // We do not set the RxE but wait for both bytes to arrive using BTF
+
+    // Document the current Status
+    periph->status = I2CReadingByte;
   }
   // Receive buffer if full, master is halted: BTF
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
@@ -496,6 +522,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
     // otherwise since there is new buffer space a new byte will be read
     PPRZ_I2C_SEND_STOP(regs);
 
+    // Document the current Status
+    periph->status = I2CStopRequested;
+
     trans->buf[0] = regs->DR;
     trans->buf[1] = regs->DR;
 
@@ -504,7 +533,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
 
     return STMI2C_SubTra_Ready_StopRequested;
   }
-  else // Hardware error
+  else // Event Logic Error
   {
     return STMI2C_SubTra_Error;
   }
@@ -512,6 +541,8 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(I2C_TypeDef *regs, st
   return STMI2C_SubTra_Busy;
 }
 
+// Doc ID 13902 Rev 11 p 712/1072
+// Transfer Sequence Diagram for Master Receiver for N>2
 static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs, struct i2c_periph *periph, struct i2c_transaction *trans)
 {
   uint16_t SR1 = regs->SR1;
@@ -525,6 +556,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     regs->CR1 |= I2C_CR1_BIT_ACK;
     // Clear the SB flag
     regs->DR = trans->slave_addr | 0x01;
+
+    // Document the current Status
+    periph->status = I2CAddrRdSent;
   }
   // Address Was Sent
   else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
@@ -541,6 +575,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // ACK is still on to get more DATA
     // Read SR2 to clear the ADDR (next byte will start arriving)
     uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+
+    // Document the current Status
+    periph->status = I2CReadingByte;
   }
   // one or more bytes are available AND we were interested in Buffer interrupts
   else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_RXNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_BIT_ITBUFEN, regs->CR2))  )
@@ -603,6 +640,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     __I2C_REG_CRITICAL_ZONE_STOP;
     // --- end of critical zone -----------
 
+    // Document the current Status
+    periph->status = I2CStopRequested;
+
     // read the byte2 we had in the buffer (BTF means 2 bytes available)
     trans->buf[periph->idx_buf] = regs->DR;
     periph->idx_buf ++;
@@ -611,371 +651,16 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(I2C_TypeDef *regs,
     // The last byte will be received with RXNE
     regs->CR2 |= I2C_CR2_BIT_ITBUFEN;
   }
-  else // Hardware error
+  else // Event Logic Error
   {
-    // Error
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED2_OFF();
-	LED1_OFF();
-#endif
     return STMI2C_SubTra_Error;
   }
 
   return STMI2C_SubTra_Busy;
 }
 
-
-static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
-{
-  uint16_t SR1 = regs->SR1;
-
-  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
-
-  //regs->CR1 &= ~ I2C_CR1_BIT_PE;		// Disable Periferial
-  //regs->CR1 |=   I2C_CR1_BIT_PE;		// Enable Periferial
-
-  // Start Condition Was Generated
-  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
-  {
-    // SB: cleared by software when reading SR1 and writing to DR
-    regs->DR = 0x00;
-  }
-  // Address Was Sent
-  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
-  {
-    // ADDR: Cleared by software when reading SR1 and then SR2
-    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
-  }
-  // Byte Transfer Finished
-  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
-  {
-    // SB: cleared by software when reading SR1 and reading/writing to DR
-    uint8_t dummy __attribute__ ((unused)) = regs->DR;
-    regs->DR = 0x00;
-  }
-
-
-  // Still have a start sheduled
-//  if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_START, regs->CR1) )
-  {
-    // Clear pending start conditions
-//    regs->CR1 &= ~ I2C_CR1_BIT_START;
-  }
-
-}
-
-
-static inline void i2c_error(struct i2c_periph *periph);
-
-static inline void i2c_irq(struct i2c_periph *periph)
-{
-
-  /*
-	There are 7 possible reasons to get here:
-
-	If IT_EV_FEN
-	-------------------------
-
-	We are always interested in all IT_EV_FEV: all are required.
-
-	1) SB		// Start Condition Success in Master mode
-	2) ADDR		// Address sent received Acknoledge
-	[3 ADDR10]	// -- 10bit address stuff
-	[4 STOPF]	// -- only for slaves: master has no stop interrupt
-	5) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
-
-	// Beware: using the buffered I2C has some interesting properties:
-	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
-           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
-	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
-	  -in master transmitmode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
-	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
-	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
-
-        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
-           a new start in one go.
-
-          -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
-           This also means you must think more in advance and a transaction could be popped from the stack even before it is
-           actually completely transmitted. But then you would not know the result yet so you have to keep it until the result
-           is known.
-
-	// Beware: the order in which Status is read determines how flags are cleared. You should not just read SR1 & SR2 every time
-
-	If IT_EV_FEN AND IT_EV_BUF
-	--------------------------
-
-	Buffer event are not always wanted and are tipically switched on during longer data transfers. Make sure to turn off in time.
-
-	6) RxNE
-	7) TxE
-
-	--------------------------------------------------------------------------------------------------
-	// This driver uses only a subset of the pprz_i2c_states for several reasons:
-	// -we have less interrupts than the I2CStatus states (for efficiency)
-	// -STM32 has such a powerfull I2C engine with plenty of status register flags that
-            only little extra status information needs to be stored.
-
-       // Status is re-used (abused) to remember the last COMMAND THAT WAS SENT to the STM I2C hardware.
-
-// TODO: check which are used
-	enum I2CStatus {
-	  I2CIdle,			// No more last command
-
-	  I2CStartRequested,		// Last command was start
-	  I2CRestartRequested,		// Last command was restart
-	  I2CStopRequested,		// Very important to not send double stop conditions
-
-	  I2CSendingByte,		// Some address/data operation
-
-	  // Following are not used
-	  I2CReadingByte,
-	  I2CAddrWrSent,
-	  I2CAddrRdSent,
-	  I2CSendingLastByte,
-	  I2CReadingLastByte,
-	  I2CComplete,
-	  I2CFailed
-	};
-
-	---------
-
-	The STM waits indefinately (holding SCL low) for user interaction:
-	a) after a master-start (waiting for address)
-	b) after an address (waiting for data)
-	   not during data sending when using buffered
-	c) after the last byte is transmitted (waiting for either stop or restart)
-	   not during data receiving when using buffered
-	   not after the last byte is received
-
-	-The STM I2C stalls indefinately when a stop condition was attempted that
-	did not succeed. The BUSY flag remains on.
-        -There is no STOP interrupt: use needs another way to finish.
-
-   */
-
-
-  ///////////////////////////////////////////////////////////////////////////////////
-  // Reading the status:
-  // - Caution: this clears several flags and can start transmissions etc...
-  // - Certain flags like STOP / (N)ACK need to be guaranteed to be set before
-  //   the transmission of the byte is finished. At higher clock rates that can be
-  //   quite fast: so we allow no other interrupt to be triggered in between
-  //   reading the status and setting all needed flags
-
-  // Direct Access to the I2C Registers
-  // Do not read SR2 as it might start the reading while an (n)ack bit might be needed first
-  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
-
-#ifdef I2C_DEBUG_LED
-  LED1_ON();
-  LED1_OFF();
-#endif
-
-
-  ///////////////////////////////////////////////////////////////////////////////////////////////////////
-  ///////////////////////////////////////////////////////////////////////////////////////////////////////
-  //
-  //	TRANSACTION HANDLER
-
-  enum STMI2CSubTransactionStatus ret = 0;
-
-  ///////////////////////
-  // Nothing Left To Do
-  if (periph->trans_extract_idx == periph->trans_insert_idx)
-  {
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED2_OFF();
-	LED1_OFF();
-
-    // no transaction and also an error?
-    LED_SHOW_ACTIVE_BITS(regs);
-#endif
-
-    // If we still get an interrupt but there are no more things to do
-    // (which can happen if an event was sheduled just before a bus error occurs)
-    // then its easy: just stop: clear all interrupt generating bits
-
-    // Count The Errors
-    i2c_error(periph);
-
-    // Clear Running Events
-    stmi2c_clear_pending_interrupts(regs);
-
-    // Mark this as a special error
-    periph->errors->unexpected_event_cnt++;
-
-    periph->status = I2CIdle;
-
-    // There are no transactions anymore:
-    // furtheron we need a transaction pointer: so we are not allowed to continue
-    return;
-  }
-
-  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
-
-  ///////////////////////////
-  // If there was an error:
-  if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
-  {
-
-#ifdef I2C_DEBUG_LED
-        LED1_ON();
-        LED2_ON();
-	LED1_OFF();
-	LED2_OFF();
-
-        LED_SHOW_ACTIVE_BITS(regs);
-#endif
-
-    // Set result in transaction
-    trans->status = I2CTransFailed;
-
-    // Prepare for next
-    ret = STMI2C_SubTra_Ready;
-
-    // Make sure a TxRx does not Restart
-    trans->type = I2CTransRx;
-
-/*
-    // There are 2 types of errors: some need a STOP, some better do without: Following will not get an extra stop
-    if (
-           // Lost Arbitration
-           (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_ARLO, regs->SR1 ) )
-           // Buss Error When Master Only
-        || ((BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_BUS,  regs->SR1 ) )  &&  (!BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL,  regs->SR2 ) ))
-        || (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ERR_OVR,  regs->SR1 ) )
-       )
-    {
-      ret = STMI2C_SubTra_Error;
-    }
-*/
-
-    // Count The Errors
-    i2c_error(periph);
-
-    // Clear Running Events
-    stmi2c_clear_pending_interrupts(regs);
-
-  }
-
-
-  ///////////////////////////
-  // Normal Event:
-  else
-  {
-
-    if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
-    {
-      switch (trans->len_r)
-      {
-        case 1:
-          ret = stmi2c_read1(regs,trans);
-          break;
-        case 2:
-          ret = stmi2c_read2(regs,trans);
-          break;
-        default:
-          ret = stmi2c_readmany(regs,periph, trans);
-          break;
-      }
-    }
-    else // TxRx or Tx
-    {
-      ret = stmi2c_send(regs,periph,trans);
-    }
-  }
-
-  /////////////////////////////////
-  // Sub-transaction has finished
-  if (ret != STMI2C_SubTra_Busy)
-  {
-    // If a restart is not needed
-    if (trans->type != I2CTransTxRx)
-    {
-      // Ready, no stop condition set yet
-      if (ret == STMI2C_SubTra_Ready)
-      {
-
-        // Program a stop
-        PPRZ_I2C_SEND_STOP(regs);
-
-        // Silent any BTF that would occur before STOP is executed
-        regs->DR = 0x00;
-      }
-
-      // In case of unexpected condition: e.g. not slave, no event
-      if (ret == STMI2C_SubTra_Error)
-      {
-
-        trans->status = I2CTransFailed;
-
-    // Error
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED2_OFF();
-	LED1_OFF();
-
-        LED_SHOW_ACTIVE_BITS(regs);
-#endif
-
-        // Clear Running Events
-        stmi2c_clear_pending_interrupts(regs);
-
-      }
-
-
-      // Jump to the next transaction
-      periph->trans_extract_idx++;
-      if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
-        periph->trans_extract_idx = 0;
-
-      // Tell everyone we are ready
-      periph->status = I2CIdle;
-
-
-      // if we have no more transaction to process, stop here
-      if (periph->trans_extract_idx == periph->trans_insert_idx)
-      {
-
-#ifdef I2C_DEBUG_LED
-        LED2_ON();
-        LED1_ON();
-	LED1_OFF();
-        LED1_ON();
-	LED1_OFF();
-	LED2_OFF();
-#endif
-      }
-      // if not, start next transaction
-      else
-      {
-        // Restart transaction doing the Rx part now
-// --- moved to idle function
-        PPRZ_I2C_SEND_START(periph);
-// ------
-     }
-
-    }
-    // RxTx -> Restart and do Rx part
-    else
-    {
-      trans->type = I2CTransRx;
-      periph->status = I2CStartRequested;
-      regs->CR1 |= I2C_CR1_BIT_START;
-
-      // Silent any BTF that would occur before SB
-      regs->DR = 0x00;
-    }
-  }
-
-  return;
-}
+////////////////////////////////////////////////
+// Restore bus conditions to normal after errors
 
 static inline void i2c_error(struct i2c_periph *periph)
 {
@@ -1022,7 +707,328 @@ static inline void i2c_error(struct i2c_periph *periph)
 #endif
 
   return;
+}
 
+
+static inline void stmi2c_clear_pending_interrupts(I2C_TypeDef *regs)
+{
+  uint16_t SR1 = regs->SR1;
+
+  // Certainly do not wait for buffer interrupts:
+  // -------------------------------------------
+  regs->CR2 &= ~ I2C_CR2_BIT_ITBUFEN;			// Disable TXE, RXNE
+
+  // Error interrupts are handled separately:
+  // ---------------------------------------
+
+  // Clear Event interrupt conditions:
+  // --------------------------------
+  
+  // Start Condition Was Generated
+  if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) )
+  {
+    // SB: cleared by software when reading SR1 and writing to DR
+    regs->DR = 0x00;
+  }
+  // Address Was Sent
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1) )
+  {
+    // ADDR: Cleared by software when reading SR1 and then SR2
+    uint16_t SR2 __attribute__ ((unused)) = regs->SR2;
+  }
+  // Byte Transfer Finished
+  if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1) )
+  {
+    // SB: cleared by software when reading SR1 and reading/writing to DR
+    uint8_t dummy __attribute__ ((unused)) = regs->DR;
+    regs->DR = 0x00;
+  }
+
+}
+
+
+////////////////////////////////////////////////
+// Restore bus conditions to normal after errors
+
+static inline void i2c_irq(struct i2c_periph *periph)
+{
+
+  /*
+	There are 7 possible event reasons to get here + all errors
+
+	If IT_EV_FEN
+	-------------------------
+
+	We are always interested in all IT_EV_FEV: all are required.
+
+	1) SB		// Start Condition Success in Master mode
+	2) ADDR		// Address sent received Acknoledge
+	[ADDR10]	// -- 10bit address stuff: not used
+	[STOPF]		// -- only for slaves: master has no stop interrupt: not used
+	3) BTF		// I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full)
+
+	// Beware: using the buffered I2C has some interesting properties:
+	  -in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is
+           in RD but the I2C can still receive a second byte. Only when the 2nd byte is received while the RxNE is 1
+	   then a BTF occurs (I2C can not continue receiving bytes or they will get lost). During BTF I2C is halted (SCL held low)
+	  -in master transmit mode: when writing a byte to WD, you instantly get a new TxE interrupt while the first is not
+	   transmitted yet. The byte was pushed to the I2C shift register and the buffer is ready for more. You can already
+	   fill new data in the buffer while the first is still being transmitted for max performance transmission.
+
+        // Beware: besides data buffering you can/must plan several consecutive actions. You can send 2 bytes to the buffer, ask for a stop and
+           a new start in one go.
+
+          -thanks to / because of this buffering and event sheduling there is not 1 interrupt per start / byte / stop
+           This also means you must think more in advance and a transaction could be popped from the transaction stack even before it's 
+           stop condition is actually generated.
+
+	// Beware: the order in which Status (and other register) is read determines how flags are cleared. 
+           You should NOT simply read SR1 & SR2 every time
+
+	If IT_EV_FEN AND IT_EV_BUF
+	--------------------------
+
+	Buffer event are not always wanted and are typically switched on during longer data transfers. Make sure to turn off in time.
+
+	4) RxNE
+	5) TxE
+
+	--------------------------------------------------------------------------------------------------
+
+	The STM waits indefinately (holding SCL low) for user interaction:
+	a) after a master-start (waiting for address)
+	b) after an address (waiting for data)
+	   not during data sending when using buffered
+	c) after the last byte is transmitted (waiting for either stop or restart)
+	   not during data receiving when using buffered
+	   not after the last byte is received
+
+	-The STM I2C stalls indefinately when a stop condition was attempted that
+	did not succeed. The BUSY flag remains on.
+        -There is no STOP interrupt.
+
+  	Caution Reading the status:
+	- Caution: this clears several flags and can start transmissions etc...
+	- Certain flags like STOP / (N)ACK need to be guaranteed to be set before
+	  the transmission of the byte is finished. At higher clock rates that can be
+	  quite fast: so we allow no other interrupt to be triggered in between
+	  reading the status and setting all needed flags
+
+   */
+
+  // Here we go ...
+
+  // Apparently we got an I2C interrupt: EVT BUF or ERR
+
+#ifdef I2C_DEBUG_LED
+  // Notify ISR is triggered
+  LED1_ON();
+  LED1_OFF();
+#endif
+
+  // Save Some Direct Access to the I2C Registers ...
+  I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+  /////////////////////////////
+  // Check if we were ready ...
+  if (periph->trans_extract_idx == periph->trans_insert_idx)
+  {
+    // Nothing Left To Do
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+
+    // no transaction and also an error?
+    LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+    // If we still get an interrupt but there are no more things to do
+    // (which can happen if an event was sheduled just before a bus error occurs)
+    // (or can happen if both error and event interrupts were called together [the 2nd will then get this error])
+
+    // since there is nothing more to do: its easy: just stop: clear all interrupt generating bits
+
+    // Count The Errors
+    i2c_error(periph);
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
+    // Mark this as a special error
+    periph->errors->last_unexpected_event++;
+
+    // Document the current Status
+    periph->status = I2CIdle;
+
+    // There are no transactions anymore: return
+    // further-on in this routine we need a transaction pointer: so we are not allowed to continue
+    return;
+  }
+
+  // get the I2C transaction we were working on ...
+
+  enum STMI2CSubTransactionStatus ret = 0;
+  struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx];
+
+  ///////////////////////////
+  // If there was an error:
+  if (( regs->SR1 & I2C_SR1_BITS_ERR ) != 0x0000)
+  {
+
+#ifdef I2C_DEBUG_LED
+        LED1_ON();
+        LED2_ON();
+	LED1_OFF();
+	LED2_OFF();
+
+        LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+    // Notify everyone about the error ...
+
+    // Set result in transaction
+    trans->status = I2CTransFailed;
+
+    // Document the current Status
+    periph->status = I2CFailed;
+
+    // Make sure a TxRx does not Restart
+    trans->type = I2CTransRx;
+
+    // Count The Errors
+    i2c_error(periph);
+
+    // Clear Running Events
+    stmi2c_clear_pending_interrupts(regs);
+
+    // Now continue as if everything was normal from now on
+    ret = STMI2C_SubTra_Ready;
+
+  }
+
+  ///////////////////////////
+  // Normal Event:
+  else
+  {
+
+    ///////////////////////////////////////////////////////////////////////////////////////////////////////
+    ///////////////////////////////////////////////////////////////////////////////////////////////////////
+    //
+    //	SUB-TRANSACTION HANDLER
+
+    if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part
+    {
+      switch (trans->len_r)
+      {
+        case 1:
+          ret = stmi2c_read1(regs,periph,trans);
+          break;
+        case 2:
+          ret = stmi2c_read2(regs,periph,trans);
+          break;
+        default:
+          ret = stmi2c_readmany(regs,periph,trans);
+          break;
+      }
+    }
+    else // TxRx or Tx
+    {
+      ret = stmi2c_send(regs,periph,trans);
+    }
+  }
+
+  /////////////////////////////////
+  // Sub-transaction has finished
+  if (ret != STMI2C_SubTra_Busy)
+  {
+    // Ready or SubTraError
+    // -ready: with or without stop already asked
+
+    // In case of unexpected event condition during subtransaction handling:
+    if (ret == STMI2C_SubTra_Error)
+    {
+      // Tell everyone about the subtransaction error:
+      // this is the previously called SPURRIOUS INTERRUPT
+      periph->status = I2CFailed;
+      trans->type = I2CTransRx;		// Avoid possible restart
+      trans->status = I2CTransFailed;	// Notify Ready
+      periph->errors->unexpected_event_cnt++;
+
+    // Error
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED2_OFF();
+	LED1_OFF();
+
+        LED_SHOW_ACTIVE_BITS(regs);
+#endif
+
+      // Clear Running Events
+      stmi2c_clear_pending_interrupts(regs);
+    }
+
+    // RxTx -> Restart and do Rx part
+    if (trans->type == I2CTransTxRx)
+    {
+      trans->type = I2CTransRx;
+      periph->status = I2CStartRequested;
+      regs->CR1 |= I2C_CR1_BIT_START;
+
+      // Silent any BTF that would occur before SB
+      regs->DR = 0x00;
+    }
+    // If a restart is not needed: Rx part or Tx-only
+    else
+    {
+      // Ready, no stop condition set yet
+      if (ret == STMI2C_SubTra_Ready)
+      {
+
+        // Program a stop
+        PPRZ_I2C_SEND_STOP(regs);
+
+        // Silent any BTF that would occur before STOP is executed
+        regs->DR = 0x00;
+      }
+
+      // Jump to the next transaction
+      periph->trans_extract_idx++;
+      if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+        periph->trans_extract_idx = 0;
+
+      // Tell everyone we are ready
+      periph->status = I2CIdle;
+
+
+      // if we have no more transaction to process, stop here
+      if (periph->trans_extract_idx == periph->trans_insert_idx)
+      {
+
+#ifdef I2C_DEBUG_LED
+        LED2_ON();
+        LED1_ON();
+	LED1_OFF();
+        LED1_ON();
+	LED1_OFF();
+	LED2_OFF();
+#endif
+      }
+      // if not, start next transaction
+      else
+      {
+        // Restart transaction doing the Rx part now
+// --- moved to idle function
+        PPRZ_I2C_SEND_START(periph);
+// ------
+     }
+    }
+  }
+
+  return;
 }
 
 
@@ -1035,14 +1041,11 @@ static inline void i2c_error(struct i2c_periph *periph)
   }
 */
 
-//#endif /* USE_I2C2 */
-
-
-
 
 #ifdef USE_I2C1
 
 struct i2c_errors i2c1_errors;
+volatile uint32_t i2c1_watchdog_counter;
 
 void i2c1_hw_init(void) {
 
@@ -1051,6 +1054,7 @@ void i2c1_hw_init(void) {
   i2c1.scl_pin = GPIO_Pin_6;
   i2c1.sda_pin = GPIO_Pin_7;
   i2c1.errors = &i2c1_errors;
+  i2c1_watchdog_counter = 0;
 
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c1_errors);
@@ -1110,6 +1114,7 @@ void i2c1_ev_irq_handler(void) {
   I2C_TypeDef *regs = (I2C_TypeDef *) i2c1.reg_addr;
   regs->CR2 &= ~ I2C_CR2_BIT_ITERREN;
   i2c_irq(&i2c1);
+  i2c1_watchdog_counter = 0;
   regs->CR2 |= I2C_CR2_BIT_ITERREN;
 }
 
@@ -1117,6 +1122,7 @@ void i2c1_er_irq_handler(void) {
   I2C_TypeDef *regs = (I2C_TypeDef *) i2c1.reg_addr;
   regs->CR2 &= ~ I2C_CR2_BIT_ITEVTEN;
   i2c_irq(&i2c1);
+  i2c1_watchdog_counter = 0;
   regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
 }
 
@@ -1125,6 +1131,7 @@ void i2c1_er_irq_handler(void) {
 #ifdef USE_I2C2
 
 struct i2c_errors i2c2_errors;
+volatile uint32_t i2c2_watchdog_counter;
 
 void i2c2_hw_init(void) {
 
@@ -1133,6 +1140,7 @@ void i2c2_hw_init(void) {
   i2c2.scl_pin = GPIO_Pin_10;
   i2c2.sda_pin = GPIO_Pin_11;
   i2c2.errors = &i2c2_errors;
+  i2c2_watchdog_counter = 0;
 
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c2_errors);
@@ -1186,6 +1194,7 @@ void i2c2_ev_irq_handler(void) {
   I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
   regs->CR2 &= ~ I2C_CR2_BIT_ITERREN;
   i2c_irq(&i2c2);
+  i2c2_watchdog_counter = 0;
   regs->CR2 |= I2C_CR2_BIT_ITERREN;
 }
 
@@ -1193,25 +1202,29 @@ void i2c2_er_irq_handler(void) {
   I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
   regs->CR2 &= ~ I2C_CR2_BIT_ITEVTEN;
   i2c_irq(&i2c2);
+  i2c2_watchdog_counter = 0;
   regs->CR2 |= I2C_CR2_BIT_ITEVTEN;
 }
 
 #endif /* USE_I2C2 */
 
+//////////////////////////////////////////////////
+// Set Bitrate to Match your application:
+// -short wires, low capacitance bus: IMU: high speed
+// -long wires with a lot of capacitance: motor controller: put speed as low as possible
 
 void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
 {
+  // If NOT Busy
   if (i2c_idle(periph))
   {
-    if (periph == &i2c2)
-    {
-      volatile int devider;
-      volatile int risetime;
+    volatile int devider;
+    volatile int risetime;
 
-      I2C_TypeDef *regs = (I2C_TypeDef *) i2c2.reg_addr;
+    I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
-      // store (just for fun)
-      I2C2_InitStruct.I2C_ClockSpeed = bitrate;
+    // store (just for fun)
+    I2C2_InitStruct.I2C_ClockSpeed = bitrate;
 
 /*****************************************************
 	Bitrate:
@@ -1230,45 +1243,52 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
       // 3) Configure rise time register
 ******************************************************/
 
-      if (bitrate < 3000)
-        bitrate = 3000;
+    if (bitrate < 3000)
+      bitrate = 3000;
 
-      // 36MHz, fast scl: 2counts low 1 count high -> / 3:
-      devider = 18000 / (bitrate/1000);
+    // 36MHz, fast scl: 2counts low 1 count high -> / 3:
+    devider = 18000 / (bitrate/1000);
 
-      // never allow faster than 600kbps
-      if (devider < 20)
-        devider = 20;
+    // never allow faster than 600kbps
+    if (devider < 20)
+      devider = 20;
 
-      // no overflow either
-      if (devider >=4095)
-        devider = 4095;
+    // no overflow either
+    if (devider >=4095)
+      devider = 4095;
 
-      risetime = 1000000 / (bitrate/1000) / 6 / 28;
+    // risetime can be up to 1/6th of the period
+    risetime = 1000000 / (bitrate/1000) / 6 / 28;
 
-      if (risetime < 10)
-        risetime = 10;
+    if (risetime < 10)
+      risetime = 10;
 
-      if (risetime >=31)
-        risetime = 31;
+    // more will overflow the register: for more you should lower the FREQ
+    if (risetime >=31)
+      risetime = 31;
 
+    // we do not expect an interrupt as the interface should have been idle, but just in case...
+    __disable_irq(); // this code is in user space: 
 
-      regs->CR1 &= ~ I2C_CR1_BIT_PE;
+    // CCR can only be written when PE is disabled
+     // p731 note 5
+    regs->CR1 &= ~ I2C_CR1_BIT_PE;
 
-      // 1)
-      regs->CR2 = 0x0324;
-      // 2)
-      //regs->CCR = 0x8000 + devider;
-      regs->CCR = 0x0000 + devider;
-      // 3)
-      regs->TRISE = risetime;
+    // 1)
+    regs->CR2 = 0x0324;
+    // 2)
+    //regs->CCR = 0x8000 + devider;
+    regs->CCR = 0x0000 + devider;
+    // 3)
+    regs->TRISE = risetime;
 
-      regs->CR1 |=   I2C_CR1_BIT_PE;
+    // Re-Enable
+    regs->CR1 |=   I2C_CR1_BIT_PE;
 
-    }
+    __enable_irq();
 
 #ifdef I2C_DEBUG_LED
-        __disable_irq();
+        __disable_irq(); // this code is in user space: 
 
         LED2_ON();
         LED1_ON();
@@ -1285,36 +1305,27 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
    }
 }
 
+
+// TODO: TODO: TODO: 
+// Watchdog timer
 void i2c_event(void)
 {
-  static uint32_t cnt = 0;
-  //I2C_TypeDef *regs;
-  cnt++;
-  if (cnt > 10000) cnt = 0;
-
-#ifndef I2C_DEBUG_LED
 #ifdef USE_I2C1
-  if (i2c1.status == I2CIdle)
-  {
-    if (i2c_idle(&i2c1))
-    {
-      __disable_irq();
-      // More work to do
-      if (i2c1.trans_extract_idx != i2c1.trans_insert_idx)
-      {
-        // Restart transaction doing the Rx part now
-        PPRZ_I2C_SEND_START(&i2c1);
-      }
-      __enable_irq();
-    }
-  }
-#endif
+  i2c1_watchdog_counter++;
 #endif
 
 #ifdef USE_I2C2
+  i2c2_watchdog_counter++;
+
+  if (i2c2_watchdog_counter > 10000)
+  {
+    i2c2.errors->timeout_tlow_cnt++;
+    i2c2_watchdog_counter = 0;
+  }
+
 
 #ifdef I2C_DEBUG_LED
-  if (cnt == 0)
+  if (i2c2_watchdog_counter == 0)
   {
         __disable_irq();
 
@@ -1417,6 +1428,9 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
 
 bool_t i2c_idle(struct i2c_periph* periph)
 {
+  // This is actually a difficult function:
+  // -simply reading the status flags can clear bits and corrupt the transaction
+
   I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
 
 #ifdef I2C_DEBUG_LED
@@ -1427,6 +1441,8 @@ bool_t i2c_idle(struct i2c_periph* periph)
         }
 #endif
 #endif
+
+  // First we check if the software thinks it is ready
   if (periph->status == I2CIdle)
     return ! (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, regs->SR2 ) );
   else
diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c
index 62acfda617..d5bee73822 100644
--- a/sw/airborne/modules/benchmark/i2c_abuse_test.c
+++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c
@@ -207,7 +207,7 @@ void event_i2c_abuse_test(void)
 		{
 			      i2c_abuse_test_counter = 1;
 
-			      //i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate);
+			      i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate);
 
 			      i2c_abuse_test_bitrate += 17000;
 			      if (i2c_abuse_test_bitrate > 410000)

From 503f4eb57b4c0ea6e2485fc0f896c123dd6582f8 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 13 Feb 2012 17:52:19 +0100
Subject: [PATCH 72/74] paparazzicenter: don't colorize strings with info in
 green, only on message

---
 sw/supervision/paparazzicenter.ml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/supervision/paparazzicenter.ml b/sw/supervision/paparazzicenter.ml
index 72ca0860a7..c3f9c47d93 100644
--- a/sw/supervision/paparazzicenter.ml
+++ b/sw/supervision/paparazzicenter.ml
@@ -207,7 +207,7 @@ let () =
 
   let errors = "red", ["error"; "no such file"; "undefined reference"; "failure"]
   and warnings = "orange", ["warning"]
-  and info = "green", ["message"; "info"] in
+  and info = "green", ["message"] in
 
   let color_regexps =
     List.map (fun (color, strings) ->

From da71b6acb8bb008e0e70191186074b2dddd9cf1f Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 13 Feb 2012 19:23:25 +0100
Subject: [PATCH 73/74] manual: switch statements

---
 doc/manual/style.dox | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/doc/manual/style.dox b/doc/manual/style.dox
index c0981cfb7f..a3aa6167ba 100644
--- a/doc/manual/style.dox
+++ b/doc/manual/style.dox
@@ -66,6 +66,30 @@ int32_t f(int32_t x1, int32_t x2)
 }
 @endcode
 
+@section styleswitch Switch statements
+
+- specify a default case
+- prefer an enum over defines for the different states
+@code
+enum state
+{
+  STATE_FOO = 1,
+  STATE_BAR = 2
+};
+
+switch( state )
+{
+  case STATE_FOO:
+    foo();
+    break;
+  case STATE_BAR:
+    bar();
+    break;
+  default:
+    break;
+}
+@endcode
+
 @section stylecpp Preprocessor directives
 - For conditional compilation use @c #if instead of @c #ifdef.
 Someone might write code like:

From 763c45a8fe22c9381f0135bbe74dfdb7133d7d7f Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 13 Feb 2012 20:42:24 +0100
Subject: [PATCH 74/74] paparazzicenter: only colorize green on "pragma
 message" string

---
 sw/supervision/paparazzicenter.ml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/supervision/paparazzicenter.ml b/sw/supervision/paparazzicenter.ml
index c3f9c47d93..308adcf55c 100644
--- a/sw/supervision/paparazzicenter.ml
+++ b/sw/supervision/paparazzicenter.ml
@@ -207,7 +207,7 @@ let () =
 
   let errors = "red", ["error"; "no such file"; "undefined reference"; "failure"]
   and warnings = "orange", ["warning"]
-  and info = "green", ["message"] in
+  and info = "green", ["pragma message"] in
 
   let color_regexps =
     List.map (fun (color, strings) ->