From 2f817471120b5885fd6b61e0e503dc08cb314520 Mon Sep 17 00:00:00 2001
From: Vince Kurtz <vjkurtz@gmail.com>
Date: Tue, 10 Apr 2018 17:07:24 -0400
Subject: [PATCH] GCS_MAVLink: fix VISP logging

log VISP messages for the ATT_POS_MOCAP mavlink message as well
---
 libraries/GCS_MAVLink/GCS.h          |  7 +++++++
 libraries/GCS_MAVLink/GCS_Common.cpp | 22 ++++++++++++++++++++--
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 385e46d1ae..2695e9a27d 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -457,6 +457,13 @@ private:
                                                       const float roll,
                                                       const float pitch,
                                                       const float yaw);
+    void _log_vision_position_estimate_data(const uint64_t usec,
+                                            const float x,
+                                            const float y,
+                                            const float z,
+                                            const float roll,
+                                            const float pitch,
+                                            const float yaw);
     void push_deferred_messages();
 
     void lock_channel(mavlink_channel_t chan, bool lock);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index a0fe927dc3..27dbf2644c 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1950,10 +1950,20 @@ void GCS_MAVLINK::_handle_common_vision_position_estimate_data(const uint64_t us
                                timestamp_ms,
                                reset_timestamp_ms);
 
-    // log data
+    _log_vision_position_estimate_data(usec, x, y, z, roll, pitch, yaw);
+}
+
+void GCS_MAVLINK::_log_vision_position_estimate_data(const uint64_t usec,
+                                                               const float x,
+                                                               const float y,
+                                                               const float z,
+                                                               const float roll,
+                                                               const float pitch,
+                                                               const float yaw)
+{
     DataFlash_Class::instance()->Log_Write("VISP", "TimeUS,PX,PY,PZ,Roll,Pitch,Yaw",
                                            "smmmrrr", "F000000", "Qffffff",
-                                           (uint64_t)timestamp_ms * 1000,
+                                           (uint64_t)usec,
                                            (double)x,
                                            (double)y,
                                            (double)z,
@@ -1987,6 +1997,14 @@ void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg)
                                angErr,
                                timestamp_ms,
                                reset_timestamp_ms);
+   
+    // calculate euler orientation for logging
+    float roll;
+    float pitch;
+    float yaw;
+    attitude.to_euler(roll, pitch, yaw);
+
+    _log_vision_position_estimate_data(m.time_usec, m.x, m.y, m.z, roll, pitch, yaw);
 }
 
 void GCS_MAVLINK::handle_command_ack(const mavlink_message_t* msg)