From 4d7beab8cc7c2826964d109486de29c993866400 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 25 Mar 2016 12:33:19 +1100
Subject: [PATCH] Plane: added QTUN logging for quadplane

---
 ArduPlane/Log.cpp       |  2 ++
 ArduPlane/defines.h     |  3 ++-
 ArduPlane/quadplane.cpp | 18 ++++++++++++++++++
 ArduPlane/quadplane.h   | 15 ++++++++++++++-
 4 files changed, 36 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp
index e434506c52..fbb6d8b8a8 100644
--- a/ArduPlane/Log.cpp
+++ b/ArduPlane/Log.cpp
@@ -500,6 +500,8 @@ static const struct LogStructure log_structure[] = {
       "ATRP", "QBBcfff",  "TimeUS,Type,State,Servo,Demanded,Achieved,P" },
     { LOG_STATUS_MSG, sizeof(log_Status),
       "STAT", "QBfBBBBBB",  "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit" },
+    { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
+      "QTUN", "Qhfffehh", "TimeUS,AngBst,ThrOut,DAlt,Alt,BarAlt,DCRt,CRt" },
 #if OPTFLOW == ENABLED
     { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
       "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 04faf3f394..bbf35e2b5d 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -116,7 +116,8 @@ enum log_messages {
     LOG_SONAR_MSG,
     LOG_ARM_DISARM_MSG,
     LOG_STATUS_MSG,
-    LOG_OPTFLOW_MSG
+    LOG_OPTFLOW_MSG,
+    LOG_QTUN_MSG
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 5442984189..a6559f435d 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -915,6 +915,7 @@ void QuadPlane::motors_output(void)
 {
     motors->output();
     plane.DataFlash.Log_Write_Rate(plane.ahrs, *motors, *attitude_control, *pos_control);
+    Log_Write_QControl_Tuning();
 }
 
 /*
@@ -1301,3 +1302,20 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
     check_land_complete();
     return false;
 }
+
+// Write a control tuning packet
+void QuadPlane::Log_Write_QControl_Tuning()
+{
+    struct log_QControl_Tuning pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG),
+        time_us             : AP_HAL::micros64(),
+        angle_boost         : attitude_control->angle_boost(),
+        throttle_out        : motors->get_throttle(),
+        desired_alt         : pos_control->get_alt_target() / 100.0f,
+        inav_alt            : inertial_nav.get_altitude() / 100.0f,
+        baro_alt            : (int32_t)plane.barometer.get_altitude() * 100,
+        desired_climb_rate  : (int16_t)pos_control->get_vel_target_z(),
+        climb_rate          : (int16_t)inertial_nav.get_velocity_z()
+    };
+    plane.DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 3186981f46..6377bb73be 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -56,7 +56,19 @@ public:
     uint8_t throttle_percentage(void) const {
         return last_throttle * 0.1f;
     }
-    
+
+    struct PACKED log_QControl_Tuning {
+        LOG_PACKET_HEADER;
+        uint64_t time_us;
+        int16_t  angle_boost;
+        float    throttle_out;
+        float    desired_alt;
+        float    inav_alt;
+        int32_t  baro_alt;
+        int16_t  desired_climb_rate;
+        int16_t  climb_rate;
+    };
+        
 private:
     AP_AHRS_NavEKF &ahrs;
     AP_Vehicle::MultiCopter aparm;
@@ -129,6 +141,7 @@ private:
 
     bool should_relax(void);
     void motors_output(void);
+    void Log_Write_QControl_Tuning();
     
     // setup correct aux channels for frame class
     void setup_default_channels(uint8_t num_motors);