From db1a066f43e6857a387ace98a471e84eafbf60a4 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Thu, 18 Dec 2014 11:55:11 -0500
Subject: [PATCH] DataFlash: Add common-vehicle Attitude logging message.

---
 libraries/DataFlash/DataFlash.h | 19 ++++++++++++++++++-
 libraries/DataFlash/LogFile.cpp | 18 ++++++++++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 90c71ed4c1..aaf093e4b6 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -79,6 +79,7 @@ public:
     void Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc);
     void Log_Write_ESC(void);
     void Log_Write_Airspeed(AP_Airspeed &airspeed);
+    void Log_Write_Attitude(AP_AHRS &ahrs, Vector3f targets);
 
     bool logging_started(void) const { return log_write_started; }
 
@@ -405,6 +406,19 @@ struct PACKED log_Camera {
     uint16_t yaw;
 };
 
+struct PACKED log_Attitude {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t  control_roll;
+    int16_t  roll;
+    int16_t  control_pitch;
+    int16_t  pitch;
+    uint16_t control_yaw;
+    uint16_t yaw;
+    uint16_t error_rp;
+    uint16_t error_yaw;
+};
+
 /*
   terrain log structure
  */
@@ -548,7 +562,9 @@ struct PACKED log_AIRSPEED {
     { LOG_ESC8_MSG, sizeof(log_Esc), \
       "ESC8",  "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
     { LOG_EKF5_MSG, sizeof(log_EKF5), \
-      "EKF5","IBhhhcccCC","TimeMS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }
+      "EKF5","IBhhhcccCC","TimeMS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
+	{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
+	  "ATT", "IccccCCCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" }
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@@ -596,6 +612,7 @@ struct PACKED log_AIRSPEED {
 #define LOG_EKF5_MSG      162
 #define LOG_BAR2_MSG	  163
 #define LOG_ARSP_MSG      164
+#define LOG_ATTITUDE_MSG  165
 
 // 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 d659717767..63434b4b5d 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -1113,6 +1113,24 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c
     WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write an attitude packet
+void DataFlash_Class::Log_Write_Attitude(AP_AHRS &ahrs, Vector3f targets)
+{
+    struct log_Attitude pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
+        time_ms         : hal.scheduler->millis(),
+        control_roll    : (int16_t)targets.x,
+        roll            : (int16_t)ahrs.roll_sensor,
+        control_pitch   : (int16_t)targets.y,
+        pitch           : (int16_t)ahrs.pitch_sensor,
+        control_yaw     : (uint16_t)targets.z,
+        yaw             : (uint16_t)ahrs.yaw_sensor,
+        error_rp        : (uint16_t)(ahrs.get_error_rp() * 100),
+        error_yaw       : (uint16_t)(ahrs.get_error_yaw() * 100)
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
 // Write ESC status messages
 void DataFlash_Class::Log_Write_ESC(void)
 {