From b85001bf4a67d56fec033c9d5c9fd76cc497831d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 6 Jan 2015 08:37:23 +1100
Subject: [PATCH] DataFlash: added logging of 2nd baro

---
 libraries/DataFlash/DataFlash.h |  3 +++
 libraries/DataFlash/LogFile.cpp | 22 ++++++++++++++++++----
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 535e759c3f..b5110cecd4 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -478,6 +478,8 @@ struct PACKED log_Esc {
       "RCOU",  "Ihhhhhhhhhhhh",     "TimeMS,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Ch8,Ch9,Ch10,Ch11,Ch12" }, \
     { LOG_BARO_MSG, sizeof(log_BARO), \
       "BARO",  "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \
+    { LOG_BAR2_MSG, sizeof(log_BARO), \
+      "BAR2",  "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \
     { LOG_POWR_MSG, sizeof(log_POWR), \
       "POWR","ICCH","TimeMS,Vcc,VServo,Flags" },  \
     { LOG_CMD_MSG, sizeof(log_Cmd), \
@@ -578,6 +580,7 @@ struct PACKED log_Esc {
 #define LOG_ESC7_MSG      160
 #define LOG_ESC8_MSG      161
 #define LOG_EKF5_MSG      162
+#define LOG_BAR2_MSG	  163
 
 // message types 200 to 210 reversed for GPS driver use
 // message types 211 to 220 reversed for autotune use
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 874fe3abc4..bf408cfe0d 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -765,15 +765,29 @@ void DataFlash_Class::Log_Write_RCOUT(void)
 // Write a BARO packet
 void DataFlash_Class::Log_Write_Baro(AP_Baro &baro)
 {
+    uint32_t now = hal.scheduler->millis();
     struct log_BARO pkt = {
         LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
-        timestamp     : hal.scheduler->millis(),
-        altitude      : baro.get_altitude(),
-        pressure	  : baro.get_pressure(),
-        temperature   : (int16_t)(baro.get_temperature() * 100),
+        timestamp     : now,
+        altitude      : baro.get_altitude(0),
+        pressure	  : baro.get_pressure(0),
+        temperature   : (int16_t)(baro.get_temperature(0) * 100),
         climbrate     : baro.get_climb_rate()
     };
     WriteBlock(&pkt, sizeof(pkt));
+#if BARO_MAX_INSTANCES > 1
+    if (baro.num_instances() > 1 && baro.healthy(1)) {
+        struct log_BARO pkt2 = {
+            LOG_PACKET_HEADER_INIT(LOG_BAR2_MSG),
+            timestamp     : now,
+            altitude      : baro.get_altitude(1),
+            pressure	  : baro.get_pressure(1),
+            temperature   : (int16_t)(baro.get_temperature(1) * 100),
+            climbrate     : baro.get_climb_rate()
+        };
+        WriteBlock(&pkt2, sizeof(pkt2));        
+    }
+#endif
 }
 
 // Write an raw accel/gyro data packet