From 0da38ba2bf3cd66e378ea32a57a7e2b7ffa8f2b6 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 28 Aug 2015 17:14:02 +0900
Subject: [PATCH] Copter: add Precision Landing log message

---
 ArduCopter/Copter.h  |  1 +
 ArduCopter/Log.cpp   | 42 ++++++++++++++++++++++++++++++++++++++++++
 ArduCopter/defines.h |  1 +
 3 files changed, 44 insertions(+)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index dca5d61062..a1daf1effd 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -632,6 +632,7 @@ private:
 #if FRAME_CONFIG == HELI_FRAME
     void Log_Write_Heli(void);
 #endif
+    void Log_Write_Precland();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
     void start_logging() ;
     void load_parameters(void);
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index a0f81105d9..89e5eab68a 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -681,6 +681,46 @@ void Copter::Log_Write_Heli()
 }
 #endif
 
+// precision landing logging
+struct PACKED log_Precland {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t healthy;
+    float bf_angle_x;
+    float bf_angle_y;
+    float ef_angle_x;
+    float ef_angle_y;
+    float pos_x;
+    float pos_y;
+};
+
+// Write an optical flow packet
+void Copter::Log_Write_Precland()
+{
+ #if PRECISION_LANDING == ENABLED
+    // exit immediately if not enabled
+    if (!precland.enabled()) {
+        return;
+    }
+
+    const Vector2f &bf_angle = precland.last_bf_angle_to_target();
+    const Vector2f &ef_angle = precland.last_ef_angle_to_target();
+    const Vector3f &target_pos_ofs = precland.last_target_pos_offset();
+    struct log_Precland pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG),
+        time_us         : hal.scheduler->micros64(),
+        healthy         : precland.healthy(),
+        bf_angle_x      : degrees(bf_angle.x),
+        bf_angle_y      : degrees(bf_angle.y),
+        ef_angle_x      : degrees(ef_angle.x),
+        ef_angle_y      : degrees(ef_angle.y),
+        pos_x           : target_pos_ofs.x,
+        pos_y           : target_pos_ofs.y
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+ #endif     // PRECISION_LANDING == ENABLED
+}
+
 const struct LogStructure Copter::log_structure[] PROGMEM = {
     LOG_COMMON_STRUCTURES,
 #if AUTOTUNE_ENABLED == ENABLED
@@ -721,6 +761,8 @@ const struct LogStructure Copter::log_structure[] PROGMEM = {
       "ERR",   "QBB",         "TimeUS,Subsys,ECode" },
     { LOG_HELI_MSG, sizeof(log_Heli),
       "HELI",  "Qhh",         "TimeUS,DRRPM,ERRPM" },
+    { LOG_PRECLAND_MSG, sizeof(log_Precland),
+      "PL",    "QBffffff",    "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
 };
 
 #if CLI_ENABLED == ENABLED
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 25fb9d9850..d1120cfb7b 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -250,6 +250,7 @@ enum FlipState {
 #define LOG_MOTBATT_MSG                 0x1E
 #define LOG_PARAMTUNE_MSG               0x1F
 #define LOG_HELI_MSG                    0x20
+#define LOG_PRECLAND_MSG                0x21
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)