From 308adb750286b1bfade0c0d86d67aebfa9fc2733 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Fri, 23 Aug 2013 17:20:53 +0200
Subject: [PATCH] [baro] ms5611: actually write result to baro.absolute

---
 sw/airborne/peripherals/ms5611_i2c.c             | 10 +++++++---
 sw/airborne/peripherals/ms5611_spi.c             | 10 +++++++---
 sw/airborne/subsystems/sensors/baro_ms5611_i2c.c |  1 +
 sw/airborne/subsystems/sensors/baro_ms5611_spi.c |  1 +
 4 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c
index 17f603d421..ae7da64d63 100644
--- a/sw/airborne/peripherals/ms5611_i2c.c
+++ b/sw/airborne/peripherals/ms5611_i2c.c
@@ -49,6 +49,8 @@ void ms5611_i2c_init(struct Ms5611_I2c *ms, struct i2c_periph *i2c_p, uint8_t ad
 void ms5611_i2c_start_configure(struct Ms5611_I2c *ms)
 {
   if (ms->status == MS5611_STATUS_UNINIT) {
+    ms->initialized = FALSE;
+    ms->prom_cnt = 0;
     ms->i2c_trans.buf[0] = MS5611_SOFT_RESET;
     i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
     ms->status = MS5611_STATUS_RESET;
@@ -152,10 +154,13 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) {
   }
   else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
     switch (ms->i2c_trans.status) {
+
       case I2CTransFailed:
         /* try again */
-        ms->prom_cnt = 0;
         ms->status = MS5611_STATUS_UNINIT;
+        ms->i2c_trans.status = I2CTransDone;
+        break;
+
       case I2CTransSuccess:
         if (ms->status == MS5611_STATUS_PROM) {
           /* read prom data */
@@ -174,14 +179,13 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) {
             }
             else {
               /* checksum error, try again */
-              ms->prom_cnt = 0;
               ms->status = MS5611_STATUS_UNINIT;
             }
           }
         }
-      case I2CTransDone:
         ms->i2c_trans.status = I2CTransDone;
         break;
+
       default:
         break;
     }
diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c
index bcb157ffd6..a183163969 100644
--- a/sw/airborne/peripherals/ms5611_spi.c
+++ b/sw/airborne/peripherals/ms5611_spi.c
@@ -63,6 +63,8 @@ void ms5611_spi_init(struct Ms5611_Spi *ms, struct spi_periph *spi_p, uint8_t sl
 void ms5611_spi_start_configure(struct Ms5611_Spi *ms)
 {
   if (ms->status == MS5611_STATUS_UNINIT) {
+    ms->initalized = FALSE;
+    ms->prom_cnt = 0;
     ms->tx_buf[0] = MS5611_SOFT_RESET;
     spi_submit(ms->spi_p, &(ms->spi_trans));
     ms->status = MS5611_STATUS_RESET;
@@ -166,10 +168,13 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) {
   }
   else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
     switch (ms->spi_trans.status) {
+
       case SPITransFailed:
         /* try again */
-        ms->prom_cnt = 0;
         ms->status = MS5611_STATUS_UNINIT;
+        ms->spi_trans.status = SPITransDone;
+        break;
+
       case SPITransSuccess:
         if (ms->status == MS5611_STATUS_PROM) {
           /* read prom data */
@@ -188,14 +193,13 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) {
             }
             else {
               /* checksum error, try again */
-              ms->prom_cnt = 0;
               ms->status = MS5611_STATUS_UNINIT;
             }
           }
         }
-      case SPITransDone:
         ms->spi_trans.status = SPITransDone;
         break;
+
       default:
         break;
     }
diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c
index a7aa03370d..a996cee2dc 100644
--- a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c
+++ b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c
@@ -100,6 +100,7 @@ void baro_event(void (*b_abs_handler)(void)){
     ms5611_i2c_event(&baro_ms5611);
 
     if (baro_ms5611.data_available) {
+      baro.absolute = baro_ms5611.data.pressure;
       b_abs_handler();
       baro_ms5611.data_available = FALSE;
 
diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c
index 127f07c31c..a86ab818f7 100644
--- a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c
+++ b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c
@@ -98,6 +98,7 @@ void baro_event(void (*b_abs_handler)(void)){
     ms5611_spi_event(&baro_ms5611);
 
     if (baro_ms5611.data_available) {
+      baro.absolute = baro_ms5611.data.pressure;
       b_abs_handler();
       baro_ms5611.data_available = FALSE;