From a144e79a50ad760e23e1929fdb0376a76464b727 Mon Sep 17 00:00:00 2001
From: Eugene Shamaev <Eugene.Shamaev@gmail.com>
Date: Sun, 9 Apr 2017 14:17:17 +0300
Subject: [PATCH] DataFlash: logging of AOA and SSA

---
 libraries/DataFlash/DataFlash.h    |  1 +
 libraries/DataFlash/LogFile.cpp    | 13 +++++++++++++
 libraries/DataFlash/LogStructure.h |  8 ++++++++
 3 files changed, 22 insertions(+)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index a8a75e5fbb..840443725f 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -150,6 +150,7 @@ public:
                         const AC_PosControl &pos_control);
     void Log_Write_Rally(const AP_Rally &rally);
     void Log_Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
+    void Log_Write_AOA_SSA(AP_AHRS &ahrs);
 
     void Log_Write(const char *name, const char *labels, const char *fmt, ...);
 
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index bc531e9c64..7bdb7801e3 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -2144,3 +2144,16 @@ void DataFlash_Class::Log_Write_VisualOdom(float time_delta, const Vector3f &ang
     };
     WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
 }
+
+// Write AOA and SSA
+void DataFlash_Class::Log_Write_AOA_SSA(AP_AHRS &ahrs)
+{
+    struct log_AOA_SSA aoa_ssa = {
+        LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),
+        time_us         : AP_HAL::micros64(),
+        AOA             : ahrs.getAOA(),
+        SSA             : ahrs.getSSA()
+    };
+
+    WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
+}
diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h
index 8ad5a98ae9..6c6f40dc2c 100644
--- a/libraries/DataFlash/LogStructure.h
+++ b/libraries/DataFlash/LogStructure.h
@@ -787,6 +787,13 @@ struct PACKED log_Rally {
     int16_t altitude;
 };
 
+struct PACKED log_AOA_SSA {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float AOA;
+    float SSA;
+};
+
 // #endif // SBP_HW_LOGGING
 
 #define ACC_LABELS "TimeUS,SampleUS,AccX,AccY,AccZ"
@@ -1183,6 +1190,7 @@ enum LogMessages {
     LOG_RATE_MSG,
     LOG_RALLY_MSG,
     LOG_VISUALODOM_MSG,
+    LOG_AOA_SSA_MSG,
 };
 
 enum LogOriginType {