From 032e8348496a810a5f8756ea989c09b0327ea637 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 4 Jul 2016 11:14:26 +0900
Subject: [PATCH] Dataflash: log rally points

---
 libraries/DataFlash/DataFlash.h    |  2 ++
 libraries/DataFlash/LogFile.cpp    | 20 ++++++++++++++++++++
 libraries/DataFlash/LogStructure.h | 15 ++++++++++++++-
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index bc689396c9..b279d7fc20 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -21,6 +21,7 @@
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <DataFlash/LogStructure.h>
 #include <AP_Motors/AP_Motors.h>
+#include <AP_Rally/AP_Rally.h>
 #include <stdint.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@@ -149,6 +150,7 @@ public:
                         const AP_Motors &motors,
                         const AC_AttitudeControl &attitude_control,
                         const AC_PosControl &pos_control);
+    void Log_Write_Rally(const AP_Rally &rally);
 
     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 9db1b786a0..78786d5916 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -1877,3 +1877,23 @@ void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs,
     };
     WriteBlock(&pkt_rate, sizeof(pkt_rate));
 }
+
+// Write rally points
+void DataFlash_Class::Log_Write_Rally(const AP_Rally &rally)
+{
+    RallyLocation rally_point;
+    for (uint8_t i=0; i<rally.get_rally_total(); i++) {
+        if (rally.get_rally_point_with_index(i, rally_point)) {
+            struct log_Rally pkt_rally = {
+                LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
+                time_us         : AP_HAL::micros64(),
+                total           : rally.get_rally_total(),
+                sequence        : i,
+                latitude        : rally_point.lat,
+                longitude       : rally_point.lng,
+                altitude        : rally_point.alt
+            };
+            WriteBlock(&pkt_rally, sizeof(pkt_rally));
+        }
+    }
+}
diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h
index 73fc7f1340..9cba7a3c75 100644
--- a/libraries/DataFlash/LogStructure.h
+++ b/libraries/DataFlash/LogStructure.h
@@ -686,6 +686,16 @@ struct PACKED log_SbpRAW2 {
     uint8_t data2[192];
 };
 
+struct PACKED log_Rally {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t total;
+    uint8_t sequence;
+    int32_t latitude;
+    int32_t longitude;
+    int16_t altitude;
+};
+
 // #endif // SBP_HW_LOGGING
 
 /*
@@ -887,7 +897,9 @@ Format characters in the format string for binary log messages
     { LOG_GIMBAL3_MSG, sizeof(log_Gimbal3), \
       "GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }, \
     { LOG_RATE_MSG, sizeof(log_Rate), \
-      "RATE", "Qffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }
+      "RATE", "Qffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }, \
+    { LOG_RALLY_MSG, sizeof(log_Rally), \
+      "RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt" }
 
 // #if SBP_HW_LOGGING
 #define LOG_SBP_STRUCTURES \
@@ -1002,6 +1014,7 @@ enum LogMessages {
     LOG_GIMBAL2_MSG,
     LOG_GIMBAL3_MSG,
     LOG_RATE_MSG,
+    LOG_RALLY_MSG,
 };
 
 enum LogOriginType {