From 6ed21c12dba2a42e18e13e72257d4f963e850da1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 10 Dec 2013 13:35:16 +1100
Subject: [PATCH] Plane: added logging of 2nd compass

---
 ArduPlane/Log.pde   | 19 +++++++++++++++++++
 ArduPlane/defines.h |  1 +
 2 files changed, 20 insertions(+)

diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index 7f4e5cd5f0..c2e055605b 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -468,6 +468,23 @@ static void Log_Write_Compass()
         offset_z        : (int16_t)mag_offsets.z
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#if COMPASS_MAX_INSTANCES > 1
+    if (compass.get_count() > 1) {
+        const Vector3f &mag2_offsets = compass.get_offsets(1);
+        const Vector3f &mag2 = compass.get_field(1);
+        struct log_Compass pkt2 = {
+            LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
+            time_ms         : hal.scheduler->millis(),
+            mag_x           : (int16_t)mag2.x,
+            mag_y           : (int16_t)mag2.y,
+            mag_z           : (int16_t)mag2.z,
+            offset_x        : (int16_t)mag2_offsets.x,
+            offset_y        : (int16_t)mag2_offsets.y,
+            offset_z        : (int16_t)mag2_offsets.z
+        };
+        DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
+    }
+#endif
 }
 
 static void Log_Write_GPS(void)
@@ -510,6 +527,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "CURR", "IhhhHf",      "TimeMS,Thr,Volt,Curr,Vcc,CurrTot" },
     { LOG_COMPASS_MSG, sizeof(log_Compass),             
       "MAG", "Ihhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
+    { LOG_COMPASS2_MSG, sizeof(log_Compass),             
+      "MAG2", "Ihhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
     TECS_LOG_FORMAT(LOG_TECS_MSG),
 };
 
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index e99627cff5..8a3f1be658 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -164,6 +164,7 @@ enum log_messages {
     LOG_TECS_MSG,
     LOG_RC_MSG,
     LOG_SONAR_MSG,
+    LOG_COMPASS2_MSG,
     MAX_NUM_LOGS // always at the end
 };