From f3461d182f7b9c9cd12f633ea3360e7800181dc2 Mon Sep 17 00:00:00 2001
From: stefanlynka <stefanlynka@yahoo.ca>
Date: Thu, 9 Jun 2016 15:33:07 +0900
Subject: [PATCH] Tracker: Added vehicle position log

---
 AntennaTracker/Log.cpp      | 28 +++++++++++++++++++++++++++-
 AntennaTracker/Tracker.h    |  1 +
 AntennaTracker/defines.h    |  1 +
 AntennaTracker/tracking.cpp |  2 +-
 4 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp
index 3473888251..471530c8c6 100644
--- a/AntennaTracker/Log.cpp
+++ b/AntennaTracker/Log.cpp
@@ -42,18 +42,44 @@ void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude)
         time_us         : AP_HAL::micros64(),
         press           : pressure,
         alt_diff        : altitude
-
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+struct PACKED log_Vehicle_Pos {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    int32_t vehicle_lat;
+    int32_t vehicle_lng;
+    int32_t vehicle_alt;
+    float vehicle_heading;
+    float vehicle_speed;
+};
+
+// Write a vehicle pos packet
+void Tracker::Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt,float heading,float ground_speed){
+
+    struct log_Vehicle_Pos pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG),
+        time_us         : AP_HAL::micros64(),
+        vehicle_lat		: lat,
+        vehicle_lng		: lng,
+        vehicle_alt		: alt,
+        vehicle_heading	: heading,
+        vehicle_speed	: ground_speed,
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
 
 const struct LogStructure Tracker::log_structure[] = {
     LOG_COMMON_STRUCTURES,
     {LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
     	      "VBAR", "Qff", "TimeUS,Press,AltDiff" },
+    {LOG_V_POS_MSG, sizeof(log_Vehicle_Pos),
+    	      "VPOS", "QLLeff", "TimeUS,Lat,Lng,Alt,Heading,Speed" }
 };
 
+
 void Tracker::Log_Write_Vehicle_Startup_Messages()
 {
     DataFlash.Log_Write_Mode(control_mode);
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index 61bdc54a6b..30c22b070e 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -255,6 +255,7 @@ private:
     void compass_cal_update();
     void Log_Write_Attitude();
     void Log_Write_Baro(void);
+    void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt,float heading,float ground_speed);
     void Log_Write_Vehicle_Baro(float pressure, float altitude);
     void Log_Write_Vehicle_Startup_Messages();
     void start_logging();
diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h
index eee3025cc4..bfcb30ad3b 100644
--- a/AntennaTracker/defines.h
+++ b/AntennaTracker/defines.h
@@ -35,3 +35,4 @@ enum ServoType {
 
 //  Logging messages
 #define LOG_V_BAR_MSG                   0x04
+#define LOG_V_POS_MSG                   0x05
diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
index 5bb7d9e25f..6ad4a75192 100644
--- a/AntennaTracker/tracking.cpp
+++ b/AntennaTracker/tracking.cpp
@@ -131,7 +131,7 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
 
     // log vehicle as GPS2
     if (should_log(MASK_LOG_GPS)) {
-        DataFlash.Log_Write_GPS(gps, 1);
+        Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.heading, vehicle.ground_speed);
     }
 }