Merge branch 'px4dev_new_param' of github.com:PX4/Firmware into px4dev_new_param

6 files changed, 286 insertions, 307 deletions

diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp
index 8489db4bb..e403ce42b 100644
--- a/apps/drivers/device/i2c.cpp
+++ b/apps/drivers/device/i2c.cpp
@@ -53,6 +53,7 @@ I2C::I2C(const char *name,
 	CDev(name, devname, irq),
 	// public
 	// protected
+	_retries(0),
 	// private
 	_bus(bus),
 	_address(address),
@@ -117,33 +118,44 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
 	struct i2c_msg_s msgv[2];
 	unsigned msgs;
 	int ret;
+	unsigned tries;
 
-//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+	do {
+	//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
 
-	msgs = 0;
+		msgs = 0;
 
-	if (send_len > 0) {
-		msgv[msgs].addr = _address;
-		msgv[msgs].flags = 0;
-		msgv[msgs].buffer = send;
-		msgv[msgs].length = send_len;
-		msgs++;
-	}
+		if (send_len > 0) {
+			msgv[msgs].addr = _address;
+			msgv[msgs].flags = 0;
+			msgv[msgs].buffer = send;
+			msgv[msgs].length = send_len;
+			msgs++;
+		}
 
-	if (recv_len > 0) {
-		msgv[msgs].addr = _address;
-		msgv[msgs].flags = I2C_M_READ;
-		msgv[msgs].buffer = recv;
-		msgv[msgs].length = recv_len;
-		msgs++;
-	}
+		if (recv_len > 0) {
+			msgv[msgs].addr = _address;
+			msgv[msgs].flags = I2C_M_READ;
+			msgv[msgs].buffer = recv;
+			msgv[msgs].length = recv_len;
+			msgs++;
+		}
+
+		if (msgs == 0)
+			return -EINVAL;
 
-	if (msgs == 0)
-		return -EINVAL;
+		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
 
-	ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
+		if (ret == OK)
+			break;
+		
+		// reset the I2C bus to unwedge on error
+		up_i2creset(_dev);
+
+	} while (tries++ < _retries);
 
 	return ret;
+
 }
 
 } // namespace device
\ No newline at end of file
diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h
index d84f7bd09..7c5a14d6b 100644
--- a/apps/drivers/device/i2c.h
+++ b/apps/drivers/device/i2c.h
@@ -53,6 +53,12 @@ class __EXPORT I2C : public CDev
 
 protected:
 	/**
+	 * The number of times a read or write operation will be retried on
+	 * error.
+	 */
+	unsigned		_retries;
+
+	/**
 	 * @ Constructor
 	 *
 	 * @param name		Driver name
diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c
index d02115a15..cca7a24aa 100644
--- a/nuttx/arch/arm/src/stm32/stm32_i2c.c
+++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c
@@ -196,11 +196,29 @@ struct stm32_trace_s
   uint32_t time;               /* First of event or first status */
 };
 
+/* I2C Device hardware configuration */
+
+struct stm32_i2c_config_s
+{
+  uint32_t    base;        /* I2C base address */
+#ifndef CONFIG_I2C_POLLED
+  int         ( *isr)(int, void *); /* Interrupt handler */
+#endif
+  uint32_t    clk_bit;     /* Clock enable bit */
+  uint32_t    reset_bit;   /* Reset bit */
+  uint32_t    scl_pin;     /* GPIO configuration for SCL as SCL */
+  uint32_t    scl_gpio;    /* GPIO configuration for SCL as a GPIO */
+  uint32_t    sda_pin;     /* GPIO configuration for SDA as SDA */
+  uint32_t    sda_gpio;    /* GPIO configuration for SDA as a GPIO */
+  uint32_t    ev_irq;      /* Event IRQ */
+  uint32_t    er_irq;      /* Error IRQ */
+};
+
 /* I2C Device Private Data */
 
 struct stm32_i2c_priv_s
 {
-  uint32_t    base;        /* I2C base address */
+  const struct stm32_i2c_config_s *config; /* Port configuration */
   int         refs;        /* Referernce count */
   sem_t       sem_excl;    /* Mutual exclusion semaphore */
 #ifndef CONFIG_I2C_POLLED
@@ -311,9 +329,32 @@ static int stm32_i2c_transfer(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *m
  ************************************************************************************/
 
 #ifdef CONFIG_STM32_I2C1
-struct stm32_i2c_priv_s stm32_i2c1_priv =
+# ifndef GPIO_I2C1_SCL_GPIO
+#  define  GPIO_I2C1_SCL_GPIO 0
+# endif
+# ifndef GPIO_I2C1_SDA_GPIO
+#  define  GPIO_I2C1_SDA_GPIO 0
+# endif
+
+static const struct stm32_i2c_config_s stm32_i2c1_config = 
 {
   .base       = STM32_I2C1_BASE,
+#ifndef CONFIG_I2C_POLLED
+  .isr        = stm32_i2c1_isr,
+#endif
+  .clk_bit    = RCC_APB1ENR_I2C1EN,
+  .reset_bit  = RCC_APB1RSTR_I2C1RST,
+  .scl_pin    = GPIO_I2C1_SCL,
+  .scl_gpio   = GPIO_I2C1_SCL_GPIO,
+  .sda_pin    = GPIO_I2C1_SDA,
+  .sda_gpio   = GPIO_I2C1_SDA_GPIO,
+  .ev_irq     = STM32_IRQ_I2C1EV,
+  .er_irq     = STM32_IRQ_I2C1ER
+};
+
+struct stm32_i2c_priv_s stm32_i2c1_priv =
+{
+  .config     = &stm32_i2c1_config,
   .refs       = 0,
   .intstate   = INTSTATE_IDLE,
   .msgc       = 0,
@@ -326,9 +367,32 @@ struct stm32_i2c_priv_s stm32_i2c1_priv =
 #endif
 
 #ifdef CONFIG_STM32_I2C2
-struct stm32_i2c_priv_s stm32_i2c2_priv =
+# ifndef GPIO_I2C2_SCL_GPIO
+#  define  GPIO_I2C2_SCL_GPIO 0
+# endif
+# ifndef GPIO_I2C2_SDA_GPIO
+#  define  GPIO_I2C2_SDA_GPIO 0
+# endif
+
+static const struct stm32_i2c_config_s stm32_i2c2_config = 
 {
   .base       = STM32_I2C2_BASE,
+#ifndef CONFIG_I2C_POLLED  
+  .isr        = stm32_i2c2_isr,
+#endif
+  .clk_bit    = RCC_APB1ENR_I2C2EN,
+  .reset_bit  = RCC_APB1RSTR_I2C2RST,
+  .scl_pin    = GPIO_I2C2_SCL,
+  .scl_gpio   = GPIO_I2C2_SCL_GPIO,
+  .sda_pin    = GPIO_I2C2_SDA,
+  .sda_gpio   = GPIO_I2C2_SDA_GPIO,
+  .ev_irq     = STM32_IRQ_I2C2EV,
+  .er_irq     = STM32_IRQ_I2C2ER
+};
+
+struct stm32_i2c_priv_s stm32_i2c2_priv =
+{
+  .config     = &stm32_i2c2_config,
   .refs       = 0,
   .intstate   = INTSTATE_IDLE,
   .msgc       = 0,
@@ -341,9 +405,32 @@ struct stm32_i2c_priv_s stm32_i2c2_priv =
 #endif
 
 #ifdef CONFIG_STM32_I2C3
-struct stm32_i2c_priv_s stm32_i2c3_priv =
+# ifndef GPIO_I2C3_SCL_GPIO
+#  define  GPIO_I2C3_SCL_GPIO 0
+# endif
+# ifndef GPIO_I2C3_SDA_GPIO
+#  define  GPIO_I2C3_SDA_GPIO 0
+# endif
+
+static const struct stm32_i2c_config_s stm32_i2c3_config = 
 {
   .base       = STM32_I2C3_BASE,
+#ifndef CONFIG_I2C_POLLED
+  .isr        = stm32_i2c3_isr,
+#endif
+  .clk_bit    = RCC_APB1ENR_I2C3EN,
+  .reset_bit  = RCC_APB1RSTR_I2C3RST,
+  .scl_pin    = GPIO_I2C3_SCL,
+  .scl_gpio   = GPIO_I2C3_SCL_GPIO,
+  .sda_pin    = GPIO_I2C3_SDA,
+  .sda_gpio   = GPIO_I2C3_SDA_GPIO,
+  .ev_irq     = STM32_IRQ_I2C3EV,
+  .er_irq     = STM32_IRQ_I2C3ER
+};
+
+struct stm32_i2c_priv_s stm32_i2c3_priv =
+{
+  .config     = &stm32_i2c3_config,
   .refs       = 0,
   .intstate   = INTSTATE_IDLE,
   .msgc       = 0,
@@ -355,7 +442,6 @@ struct stm32_i2c_priv_s stm32_i2c3_priv =
 };
 #endif
 
-
 /* Device Structures, Instantiation */
 
 struct i2c_ops_s stm32_i2c_ops =
@@ -391,7 +477,7 @@ struct i2c_ops_s stm32_i2c_ops =
 static inline uint16_t stm32_i2c_getreg(FAR struct stm32_i2c_priv_s *priv,
                                         uint8_t offset)
 {
-  return getreg16(priv->base + offset);
+  return getreg16(priv->config->base + offset);
 }
 
 /************************************************************************************
@@ -405,7 +491,7 @@ static inline uint16_t stm32_i2c_getreg(FAR struct stm32_i2c_priv_s *priv,
 static inline void stm32_i2c_putreg(FAR struct stm32_i2c_priv_s *priv, uint8_t offset,
                                     uint16_t value)
 {
-  putreg16(value, priv->base + offset);
+  putreg16(value, priv->config->base + offset);
 }
 
 /************************************************************************************
@@ -420,7 +506,7 @@ static inline void stm32_i2c_modifyreg(FAR struct stm32_i2c_priv_s *priv,
                                        uint8_t offset, uint16_t clearbits,
                                        uint16_t setbits)
 {
-  modifyreg16(priv->base + offset, clearbits, setbits);
+  modifyreg16(priv->config->base + offset, clearbits, setbits);
 }
 
 /************************************************************************************
@@ -986,7 +1072,7 @@ static inline uint32_t stm32_i2c_disablefsmc(FAR struct stm32_i2c_priv_s *priv)
   /* Is this I2C1 */
 
 #ifdef CONFIG_STM32_I2C2
-  if (priv->base == STM32_I2C1_BASE)
+  if (priv->config->base == STM32_I2C1_BASE)
 #endif
     {
       /* Disable FSMC unconditionally */
@@ -1310,114 +1396,35 @@ static int stm32_i2c_init(FAR struct stm32_i2c_priv_s *priv)
 {
   /* Power-up and configure GPIOs */
 
-  switch (priv->base)
-    {
-#ifdef CONFIG_STM32_I2C1
-      case STM32_I2C1_BASE:
-
-        /* Enable power and reset the peripheral */
-
-        modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN);
+  /* Enable power and reset the peripheral */
 
-        modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
-        modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
+  modifyreg32(STM32_RCC_APB1ENR, 0, priv->config->clk_bit);
 
-        /* Configure pins */
-            
-        if (stm32_configgpio(GPIO_I2C1_SCL) < 0)
-          {
-            return ERROR;
-          }
-
-        if (stm32_configgpio(GPIO_I2C1_SDA) < 0)
-          {
-            stm32_unconfiggpio(GPIO_I2C1_SCL);
-            return ERROR;
-          }
+  modifyreg32(STM32_RCC_APB1RSTR, 0, priv->config->reset_bit);
+  modifyreg32(STM32_RCC_APB1RSTR, priv->config->reset_bit, 0);
 
-        /* Attach ISRs */
-            
-#ifndef CONFIG_I2C_POLLED
-        irq_attach(STM32_IRQ_I2C1EV, stm32_i2c1_isr);
-        irq_attach(STM32_IRQ_I2C1ER, stm32_i2c1_isr);
-        up_enable_irq(STM32_IRQ_I2C1EV);
-        up_enable_irq(STM32_IRQ_I2C1ER);
-#endif
-        break;
-#endif
+  /* Configure pins */
       
-#ifdef CONFIG_STM32_I2C2
-      case STM32_I2C2_BASE:
-
-        /* Enable power and reset the peripheral */
-
-        modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C2EN);
-
-        modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C2RST);
-        modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C2RST, 0);
-
-        /* Configure pins */
-
-        if (stm32_configgpio(GPIO_I2C2_SCL) < 0)
-          {
-            return ERROR;
-          }
-
-        if (stm32_configgpio(GPIO_I2C2_SDA) < 0)
-          {
-            stm32_unconfiggpio(GPIO_I2C2_SCL);
-            return ERROR;
-          }
-
-        /* Attach ISRs */
-
-#ifndef CONFIG_I2C_POLLED
-        irq_attach(STM32_IRQ_I2C2EV, stm32_i2c2_isr);
-        irq_attach(STM32_IRQ_I2C2ER, stm32_i2c2_isr);
-        up_enable_irq(STM32_IRQ_I2C2EV);
-        up_enable_irq(STM32_IRQ_I2C2ER);
-#endif
-        break;
-#endif
-
-#ifdef CONFIG_STM32_I2C3
-      case STM32_I2C3_BASE:
-
-        /* Enable power and reset the peripheral */
-
-        modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C3EN);
-
-        modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C3RST);
-        modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C3RST, 0);
-
-        /* Configure pins */
-
-        if (stm32_configgpio(GPIO_I2C3_SCL) < 0)
-          {
-            return ERROR;
-          }
-
-        if (stm32_configgpio(GPIO_I2C3_SDA) < 0)
-          {
-            stm32_unconfiggpio(GPIO_I2C3_SCL);
-            return ERROR;
-          }
+  if (stm32_configgpio(priv->config->scl_pin) < 0)
+    {
+      return ERROR;
+    }
 
-        /* Attach ISRs */
+  if (stm32_configgpio(priv->config->sda_pin) < 0)
+    {
+      stm32_unconfiggpio(priv->config->scl_pin);
+      return ERROR;
+    }
 
+  /* Attach ISRs */
+            
 #ifndef CONFIG_I2C_POLLED
-        irq_attach(STM32_IRQ_I2C3EV, stm32_i2c3_isr);
-        irq_attach(STM32_IRQ_I2C3ER, stm32_i2c3_isr);
-        up_enable_irq(STM32_IRQ_I2C3EV);
-        up_enable_irq(STM32_IRQ_I2C3ER);
-#endif
-        break;
+        irq_attach(priv->config->ev_irq, priv->config->isr);
+        irq_attach(priv->config->er_irq, priv->config->isr);
+        up_enable_irq(priv->config->ev_irq);
+        up_enable_irq(priv->config->er_irq);
 #endif
 
-      default:
-        return ERROR;
-    }
-
   /* Set peripheral frequency, where it must be at least 2 MHz  for 100 kHz
    * or 4 MHz for 400 kHz.  This also disables all I2C interrupts.
    */
@@ -1445,56 +1452,16 @@ static int stm32_i2c_deinit(FAR struct stm32_i2c_priv_s *priv)
 
   stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, 0);
 
-  switch (priv->base)
-    {
-#ifdef CONFIG_STM32_I2C1
-      case STM32_I2C1_BASE:
-        stm32_unconfiggpio(GPIO_I2C1_SCL);
-        stm32_unconfiggpio(GPIO_I2C1_SDA);
-
-#ifndef CONFIG_I2C_POLLED
-        up_disable_irq(STM32_IRQ_I2C1EV);
-        up_disable_irq(STM32_IRQ_I2C1ER);
-        irq_detach(STM32_IRQ_I2C1EV);
-        irq_detach(STM32_IRQ_I2C1ER);
-#endif
-        modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_I2C1EN, 0);
-        break;
-#endif
-      
-#ifdef CONFIG_STM32_I2C2
-      case STM32_I2C2_BASE:
-        stm32_unconfiggpio(GPIO_I2C2_SCL);
-        stm32_unconfiggpio(GPIO_I2C2_SDA);
-
-#ifndef CONFIG_I2C_POLLED
-        up_disable_irq(STM32_IRQ_I2C2EV);
-        up_disable_irq(STM32_IRQ_I2C2ER);
-        irq_detach(STM32_IRQ_I2C2EV);
-        irq_detach(STM32_IRQ_I2C2ER);
-#endif
-        modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_I2C2EN, 0);
-        break;
-#endif
-
-#ifdef CONFIG_STM32_I2C3
-      case STM32_I2C3_BASE:
-        stm32_unconfiggpio(GPIO_I2C3_SCL);
-        stm32_unconfiggpio(GPIO_I2C3_SDA);
+  stm32_unconfiggpio(priv->config->scl_pin);
+  stm32_unconfiggpio(priv->config->sda_pin);
 
 #ifndef CONFIG_I2C_POLLED
-        up_disable_irq(STM32_IRQ_I2C3EV);
-        up_disable_irq(STM32_IRQ_I2C3ER);
-        irq_detach(STM32_IRQ_I2C3EV);
-        irq_detach(STM32_IRQ_I2C3ER);
+  up_disable_irq(priv->config->ev_irq);
+  up_disable_irq(priv->config->er_irq);
+  irq_detach(priv->config->ev_irq);
+  irq_detach(priv->config->er_irq);
 #endif
-        modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_I2C3EN, 0);
-        break;
-#endif
-        
-      default:
-        return ERROR;
-    }
+  modifyreg32(STM32_RCC_APB1ENR, priv->config->clk_bit, 0);
 
   return OK;
 }
@@ -1979,159 +1946,119 @@ int up_i2cuninitialize(FAR struct i2c_dev_s * dev)
 
 int up_i2creset(FAR struct i2c_dev_s * dev)
 {
-  // ASSERT(dev);
-  
-  // stm32_i2c_sem_wait(dev);   /* ensure that address or flags don't change meanwhile */
-
-  // struct stm32_i2c_inst_s     *inst = (struct stm32_i2c_inst_s *)dev;
-  // FAR struct stm32_i2c_priv_s *priv = inst->priv;
-
-  // /* Clear any pending error interrupts */
-
-  // stm32_i2c_putreg(priv, STM32_I2C_SR1_OFFSET, 0);
-
-  // /* "Note: When the STOP, START or PEC bit is set, the software must
-  //  *  not perform any write access to I2C_CR1 before this bit is
-  //  *  cleared by hardware. Otherwise there is a risk of setting a
-  //  *  second STOP, START or PEC request."  However, if the bits are
-  //  *  not cleared by hardware, then we will have to do that from hardware.
-  //  */
-
-  // stm32_i2c_clrstart(priv);
-
-  //   /* Reset peripheral */
-  // uint16_t cr1 = stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET);
-  // /* Set reset flag */
-  // cr1 &= I2C_CR1_SWRST;
-  // stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, cr1);
-  // /* Clear CR1 */
-  // stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, 0);
-
-  // switch (priv->base)
-  // {
-  //   case STM32_I2C1_BASE:
-  //     modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
-  //     modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
-  //     break;
-  //   case STM32_I2C2_BASE:
-  //     modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C2RST);
-  //     modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C2RST, 0);
-  //     break;
-  //   case STM32_I2C3_BASE:
-  //     modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C3RST);
-  //     modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C3RST, 0);
-  //     break;
-  // }
-
-
-
-  // stm32_i2c_putreg(priv, STM32_I2C_CR2_OFFSET, (STM32_PCLK1_FREQUENCY / 1000000));
-  // stm32_i2c_setclock(priv, inst->frequency);
-  // inst->flags = 0;
-        
-  // /* Enable I2C */
-    
-  // stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_PE);
+  struct stm32_i2c_priv_s * priv;
+  unsigned clock_count;
+  unsigned stretch_count;
+  int ret = ERROR;
+  irqstate_t state;
 
+  ASSERT(dev);
 
+  /* Get I2C private structure */
 
+  priv = ((struct stm32_i2c_inst_s *)dev)->priv;
 
+  /* Our caller must own a ref */
 
+  ASSERT(priv->refs > 0);
 
+  /* Lock out other clients */
 
+  stm32_i2c_sem_wait(dev);
 
+  /* De-init the port */
 
+  stm32_i2c_deinit(priv);
 
+  /* If possible, use GPIO configuration to un-wedge the bus */
 
+  if ((priv->config->scl_gpio != 0) && (priv->config->sda_gpio != 0))
+    {
+      stm32_configgpio(priv->config->scl_gpio);
+      stm32_configgpio(priv->config->sda_gpio);
 
+      /*
+       * Clock the bus until any slaves currently driving it let it go.
+       */
 
+      clock_count = 0;
+      while (!stm32_gpioread(priv->config->sda_gpio)) 
+        {
 
+          /* Give up if we have tried too hard */
 
-  int irqs;
-    
-  ASSERT(dev);
-    
-  /* Decrement refs and check for underflow */
-    
-  if (((struct stm32_i2c_inst_s *)dev)->priv->refs == 0) 
-    {
-      return ERROR;
-    }
+          if (clock_count++ > 1000)
+            { 
+              goto out;
+            }
 
-  irqs = irqsave();
+          /*
+           * Sniff to make sure that clock stretching has finished.
+           *
+           * If the bus never relaxes, the reset has failed.
+           */
 
-  irqrestore(irqs);
+          stretch_count = 0;
+          while (!stm32_gpioread(priv->config->scl_gpio))
+            { 
 
-  /* Disable power and other HW resource (GPIO's) */
+              /* Give up if we have tried too hard */
 
-  //stm32_i2c_deinit( ((struct stm32_i2c_inst_s *)dev)->priv );
+              if (stretch_count++ > 1000)
+                { 
+                  goto out;
+                }
 
-  /* Release unused resources */
+              up_udelay(10);
 
-  //stm32_i2c_sem_destroy( (struct i2c_dev_s *)dev );
-  
-  struct stm32_i2c_priv_s * priv = NULL;  /* private data of device with multiple instances */
-  struct stm32_i2c_inst_s * inst = NULL;  /* device, single instance */
+            }
 
-#if STM32_PCLK1_FREQUENCY < 4000000
-#   warning STM32_I2C_INIT: Peripheral clock must be at least 4 MHz to support 400 kHz operation.
-#endif
+            /* Drive SCL low */
 
-#if STM32_PCLK1_FREQUENCY < 2000000
-#   warning STM32_I2C_INIT: Peripheral clock must be at least 2 MHz to support 100 kHz operation.
-    return NULL;
-#endif
-    
-  /* Get I2C private structure */
+            stm32_gpiowrite(priv->config->scl_gpio, 0);
+            up_udelay(10);
 
-    priv = ((struct stm32_i2c_inst_s *)dev)->priv;
-
-//   switch (port)
-//     {
-// #ifdef CONFIG_STM32_I2C1
-//       case 1:
-//         priv = (struct stm32_i2c_priv_s *)&stm32_i2c1_priv;
-//         break;
-// #endif
-// #ifdef CONFIG_STM32_I2C2
-//       case 2:
-//         priv = (struct stm32_i2c_priv_s *)&stm32_i2c2_priv;
-//         break;
-// #endif
-// #ifdef CONFIG_STM32_I2C3
-//       case 3:
-//         priv = (struct stm32_i2c_priv_s *)&stm32_i2c3_priv;
-//         break;
-// #endif
-//       default:
-//         return NULL;
-//     }
+            /* Drive SCL high again */
 
-  /* Allocate instance */
-  inst = (struct stm32_i2c_inst_s *)dev;
+            stm32_gpiowrite(priv->config->scl_gpio, 1);
+            up_udelay(10);
 
-  /* Initialize instance */
+        }
 
-  inst->ops       = &stm32_i2c_ops;
-  inst->priv      = priv;
-  inst->frequency = 100000;
-  inst->address   = 0;
-  inst->flags     = 0;
+      /*
+       * Generate a start followed by a stop to reset slave
+       * state machines.
+       */
+
+      stm32_gpiowrite(priv->config->sda_gpio, 0);
+      up_udelay(10);
+      stm32_gpiowrite(priv->config->scl_gpio, 0);
+      up_udelay(10);
+      stm32_gpiowrite(priv->config->scl_gpio, 1);
+      up_udelay(10);
+      stm32_gpiowrite(priv->config->sda_gpio, 1);
+      up_udelay(10);
+
+      /*
+       * Revert the GPIO configuration.
+       */
+      stm32_unconfiggpio(priv->config->sda_gpio);
+      stm32_unconfiggpio(priv->config->scl_gpio);
 
-  /* Init private data for the first time, increment refs count,
-   * power-up hardware and configure GPIOs. 
-   */
-    
-  irqs = irqsave();
-    
-  if ((volatile int)priv->refs++ == 0)
-    {
-      stm32_i2c_sem_init( (struct i2c_dev_s *)inst );
-      stm32_i2c_init( priv );
     }
-    
-  irqrestore(irqs);
-  return OK;
+
+  /* Re-init the port */
+
+  stm32_i2c_init(priv);
+  ret = OK;
+
+out:
+
+  /* release the port for re-use by other clients */
+
+  stm32_i2c_sem_post(dev);
+
+  return ret;
 }
 
-#endif /* defined(CONFIG_STM32_I2C1) && defined(CONFIG_STM32_I2C2) */
+#endif /* defined(CONFIG_STM32_I2C1) || defined(CONFIG_STM32_I2C2) || defined(CONFIG_STM32_I2C3) */
diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h
index 29ad52c61..60a295f8d 100755
--- a/nuttx/configs/px4fmu/include/board.h
+++ b/nuttx/configs/px4fmu/include/board.h
@@ -265,15 +265,25 @@
 
 /*
  * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
  */
-#define GPIO_I2C1_SCL	GPIO_I2C1_SCL_2
-#define GPIO_I2C1_SDA	GPIO_I2C1_SDA_2
-
-#define GPIO_I2C2_SCL	GPIO_I2C2_SCL_1
-#define GPIO_I2C2_SDA	GPIO_I2C2_SDA_1
-
-#define GPIO_I2C3_SCL	GPIO_I2C3_SCL_1
-#define GPIO_I2C3_SDA	GPIO_I2C3_SDA_1
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_I2C2_SCL		GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA		GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+#define GPIO_I2C3_SCL		GPIO_I2C3_SCL_1
+#define GPIO_I2C3_SDA		GPIO_I2C3_SDA_1
+#define GPIO_I2C3_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_I2C3_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
 
 /*
  * I2C busses
diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
index df6e26d4b..d51d654f7 100644
--- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c
+++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
@@ -323,13 +323,20 @@ hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg)
 	return result;
 }
 
+extern int up_i2creset(FAR struct i2c_dev_s * dev);
+
 int hmc5883l_reset()
 {
 	int ret;
+#if 1
+	ret = up_i2creset(hmc5883l_dev.i2c);
+	printf("HMC5883: BUS RESET %s\n", ret ? "FAIL" : "OK");
+#else
 	printf("[hmc5883l drv] Resettet I2C2 BUS\n");
 	up_i2cuninitialize(hmc5883l_dev.i2c);
 	hmc5883l_dev.i2c = up_i2cinitialize(2);
 	I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000);
+#endif
 	return ret;
 }
 
diff --git a/nuttx/include/nuttx/i2c.h b/nuttx/include/nuttx/i2c.h
index b2238b1cf..ef3d9a388 100644
--- a/nuttx/include/nuttx/i2c.h
+++ b/nuttx/include/nuttx/i2c.h
@@ -325,6 +325,23 @@ EXTERN FAR struct i2c_dev_s *up_i2cinitialize(int port);
 
 EXTERN int up_i2cuninitialize(FAR struct i2c_dev_s * dev);
 
+/****************************************************************************
+ * Name: up_i2creset
+ *
+ * Description:
+ *   Reset the port and the associated I2C bus.  Useful when the bus or an
+ *   attached slave has become wedged or unresponsive.
+ *
+ * Input Parameter:
+ *   Device structure as returned by the up_i2cinitalize()
+ *
+ * Returned Value:
+ *   OK on success, ERROR if the bus cannot be unwedged. 
+ *
+ ****************************************************************************/
+
+EXTERN int up_i2creset(FAR struct i2c_dev_s * dev);
+
 #undef EXTERN
 #if defined(__cplusplus)
 }