From 0c5b393919fdb56c2f64ebc625953521796dec36 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 19 Apr 2013 23:54:40 +1000
Subject: [PATCH] Plane: added compass logging

---
 ArduPlane/ArduPlane.pde |  3 +++
 ArduPlane/Log.pde       | 36 ++++++++++++++++++++++++++++
 ArduPlane/config.h      | 52 ++++++-----------------------------------
 ArduPlane/defines.h     |  2 ++
 4 files changed, 48 insertions(+), 45 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 3eab6ca7a6..8b3d7ec896 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -814,6 +814,9 @@ static void medium_loop()
         if (g.compass_enabled && compass.read()) {
             ahrs.set_compass(&compass);
             compass.null_offsets();
+            if (g.log_bitmask & MASK_LOG_COMPASS) {
+                Log_Write_Compass();
+            }
         } else {
             ahrs.set_compass(NULL);
         }
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index 76667ede97..2a65cddc93 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -48,6 +48,7 @@ print_log_menu(void)
         PLOG(IMU);
         PLOG(CMD);
         PLOG(CURRENT);
+        PLOG(COMPASS);
  #undef PLOG
     }
 
@@ -136,6 +137,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
         TARG(IMU);
         TARG(CMD);
         TARG(CURRENT);
+        TARG(COMPASS);
  #undef TARG
     }
 
@@ -375,6 +377,40 @@ static void Log_Write_Current()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+
+struct PACKED log_Compass {
+    LOG_PACKET_HEADER;
+    int16_t mag_x;
+    int16_t mag_y;
+    int16_t mag_z;
+    int16_t offset_x;
+    int16_t offset_y;
+    int16_t offset_z;
+    int16_t motor_offset_x;
+    int16_t motor_offset_y;
+    int16_t motor_offset_z;
+};
+
+// Write a Compass packet. Total length : 15 bytes
+static void Log_Write_Compass()
+{
+    Vector3f mag_offsets = compass.get_offsets();
+    Vector3f mag_motor_offsets = compass.get_motor_offsets();
+    struct log_Compass pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
+        mag_x           : compass.mag_x,
+        mag_y           : compass.mag_y,
+        mag_z           : compass.mag_z,
+        offset_x        : (int16_t)mag_offsets.x,
+        offset_y        : (int16_t)mag_offsets.y,
+        offset_z        : (int16_t)mag_offsets.z,
+        motor_offset_x  : (int16_t)mag_motor_offsets.x,
+        motor_offset_y  : (int16_t)mag_motor_offsets.y,
+        motor_offset_z  : (int16_t)mag_motor_offsets.z
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
 static const struct LogStructure log_structure[] PROGMEM = {
     LOG_COMMON_STRUCTURES,
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index a55223c4b9..77aa0ab3e7 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -714,52 +714,14 @@
  # define LOGGING_ENABLED                ENABLED
 #endif
 
-
-#ifndef LOG_ATTITUDE_FAST
- # define LOG_ATTITUDE_FAST              DISABLED
-#endif
-#ifndef LOG_ATTITUDE_MED
- # define LOG_ATTITUDE_MED               ENABLED
-#endif
-#ifndef LOG_GPS
- # define LOG_GPS                                ENABLED
-#endif
-#ifndef LOG_PM
- # define LOG_PM                                 ENABLED
-#endif
-#ifndef LOG_CTUN
- # define LOG_CTUN                               DISABLED
-#endif
-#ifndef LOG_NTUN
- # define LOG_NTUN                               DISABLED
-#endif
-#ifndef LOG_MODE
- # define LOG_MODE                               ENABLED
-#endif
-#ifndef LOG_IMU
- # define LOG_IMU                                DISABLED
-#endif
-#ifndef LOG_CMD
- # define LOG_CMD                                ENABLED
-#endif
-#ifndef LOG_CURRENT
- # define LOG_CURRENT                            DISABLED
-#endif
-
-// calculate the default log_bitmask
-#define LOGBIT(_s)      (LOG_ ## _s ? MASK_LOG_ ## _s : 0)
-
 #define DEFAULT_LOG_BITMASK     \
-    LOGBIT(ATTITUDE_FAST)       | \
-    LOGBIT(ATTITUDE_MED)        | \
-    LOGBIT(GPS)                 | \
-    LOGBIT(PM)                  | \
-    LOGBIT(CTUN)                | \
-    LOGBIT(NTUN)                | \
-    LOGBIT(MODE)                | \
-    LOGBIT(IMU)                 | \
-    LOGBIT(CMD)                 | \
-    LOGBIT(CURRENT)
+    MASK_LOG_ATTITUDE_MED | \
+    MASK_LOG_GPS | \
+    MASK_LOG_PM | \
+    MASK_LOG_NTUN | \
+    MASK_LOG_MODE | \
+    MASK_LOG_CMD | \
+    MASK_LOG_COMPASS
 
 
 //////////////////////////////////////////////////////////////////////////////
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 1353d34832..655d8b885f 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -169,6 +169,7 @@ enum log_messages {
     LOG_CAMERA_MSG,
     LOG_ATTITUDE_MSG,
     LOG_MODE_MSG,
+    LOG_COMPASS_MSG,
     MAX_NUM_LOGS
 };
 
@@ -182,6 +183,7 @@ enum log_messages {
 #define MASK_LOG_IMU                    (1<<7)
 #define MASK_LOG_CMD                    (1<<8)
 #define MASK_LOG_CURRENT                (1<<9)
+#define MASK_LOG_COMPASS                (1<<10)
 
 // Waypoint Modes
 // ----------------