From b8ec860edc4f7b46bcbb2816f681fe1332f7fbb2 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 28 Aug 2013 14:28:27 +0200
Subject: [PATCH] [ardrone] 200Hz Baro messages are actually 100Hz temp and
 then pressure. One should not use the wrong temp with the wrong pressure
 either. A change tracker is used.

---
 sw/airborne/boards/ardrone/navdata.c | 57 +++++++++++++++++++++++++++-
 1 file changed, 56 insertions(+), 1 deletion(-)

diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 831a492367..683426faba 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/sw/airborne/boards/ardrone/navdata.c
@@ -172,6 +172,60 @@ void navdata_read()
   }
 }
 
+void baro_update_logic(void);
+void baro_update_logic(void)
+{
+  static int32_t lastpressval = 0;
+  static uint16_t lasttempval = 0;
+  static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press
+
+  static int sync_errors;
+
+  if (temp_or_press_was_updated_last == 0)  // Last update was press so we are now waiting for temp
+  {
+    // temp was updated
+    temp_or_press_was_updated_last = 1;
+    
+    // This means that press must remain constant
+    if (lastpressval != 0)
+    {
+      // If pressure was updated: this is a sync error
+      if (lastpressval != navdata->pressure)
+      {
+        // wait for temp again
+        temp_or_press_was_updated_last = 0;
+        sync_errors++;
+        navdata_baro_available = 1;
+      }
+    }
+  }
+  else
+  {
+    // press was updated
+    temp_or_press_was_updated_last = 0;
+
+    // This means that temp must remain constant
+    if (lasttempval != 0)
+    {
+      // If temp was updated: this is a sync error
+      if (lasttempval != navdata->temperature_pressure)
+      {
+        // wait for press again
+        temp_or_press_was_updated_last = 1;
+        sync_errors++;
+      }
+    }
+
+    navdata_baro_available = 1;
+  }
+
+  lastpressval = navdata->pressure;
+  lasttempval = navdata->temperature_pressure;
+
+  // debug
+  // navdata->temperature_pressure = sync_errors;
+}
+
 void navdata_update()
 {
   navdata_read();
@@ -198,8 +252,9 @@ void navdata_update()
         p[0] = p[1];
         p[1] = tmp;
 
+        baro_update_logic();
+
         navdata_imu_available = 1;
-        navdata_baro_available = 1;
 
         port->packetsRead++;
 //        printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));