GCS_MAVLink: add VISP logging

2018-03-30 15:59:33 +09:00 · 2018-03-30 15:59:33 +09:00 · 64be134f7b
parent f03c73aeff
commit 64be134f7b
1 changed files with 11 additions and 0 deletions
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@ -1925,6 +1925,17 @@ void GCS_MAVLINK::_handle_common_vision_position_estimate_data(const uint64_t us
                               angErr,
                               timestamp_ms,
                               reset_timestamp_ms);
+
+    // log data
+    DataFlash_Class::instance()->Log_Write("VISP", "TimeUS,PX,PY,PZ,Roll,Pitch,Yaw",
+                                           "smmmrrr", "F000000", "Qffffff",
+                                           (uint64_t)timestamp_ms * 1000,
+                                           (double)x,
+                                           (double)y,
+                                           (double)z,
+                                           (double)roll,
+                                           (double)pitch,
+                                           (double)yaw);
 }

 void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg)