From f18be824ea6f4a49394ba59255bf4f79e2afa9c4 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 26 Mar 2020 13:00:30 +0900
Subject: [PATCH] AP_Logger: add VISP message

---
 libraries/AP_Logger/AP_Logger.h    |  1 +
 libraries/AP_Logger/LogFile.cpp    | 18 ++++++++++++++++++
 libraries/AP_Logger/LogStructure.h | 29 +++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+)

diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 92e3ee5ff0..2e7e5734bf 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -281,6 +281,7 @@ public:
                           uint8_t sequence,
                           const RallyLocation &rally_point);
     void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
+    void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw);
     void Write_AOA_SSA(AP_AHRS &ahrs);
     void Write_Beacon(AP_Beacon &beacon);
     void Write_Proximity(AP_Proximity &proximity);
diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp
index 5778583edc..6ea69d13ff 100644
--- a/libraries/AP_Logger/LogFile.cpp
+++ b/libraries/AP_Logger/LogFile.cpp
@@ -914,6 +914,24 @@ void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta,
     WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
 }
 
+// Write visual position sensor data.  x,y,z are in meters, angles are in degrees
+void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw)
+{
+    const struct log_VisualPosition pkt_visualpos {
+        LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
+        time_us         : AP_HAL::micros64(),
+        remote_time_us  : remote_time_us,
+        time_ms         : time_ms,
+        pos_x           : x,
+        pos_y           : y,
+        pos_z           : z,
+        roll            : roll,
+        pitch           : pitch,
+        yaw             : yaw
+    };
+    WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
+}
+
 // Write AOA and SSA
 void AP_Logger::Write_AOA_SSA(AP_AHRS &ahrs)
 {
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index 0df09b19b9..694d867688 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -650,6 +650,20 @@ struct PACKED log_VisualOdom {
     float confidence;
 };
 
+// visual position data
+struct PACKED log_VisualPosition {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint64_t remote_time_us;
+    uint32_t time_ms;
+    float pos_x;
+    float pos_y;
+    float pos_z;
+    float roll;     // degrees
+    float pitch;    // degrees
+    float yaw;      // degrees
+};
+
 struct PACKED log_ekfBodyOdomDebug {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1874,6 +1888,18 @@ struct PACKED log_Arm_Disarm {
 // @Field: Clip1: Number of clipping events on 2nd accelerometer
 // @Field: Clip2: Number of clipping events on 3rd accelerometer
 
+// @LoggerMessage: VISP
+// @Description: Vision Position
+// @Field: TimeUS: System time
+// @Field: RemTimeUS: Remote system time
+// @Field: CTimeMS: Corrected system time
+// @Field: PX: Position X-axis (North-South)
+// @Field: PY: Position Y-axis (East-West)
+// @Field: PZ: Position Z-axis (Down-Up)
+// @Field: Roll: Roll lean angle
+// @Field: Pitch: Pitch lean angle
+// @Field: Yaw: Yaw angle
+
 // messages for all boards
 #define LOG_BASE_STRUCTURES \
     { LOG_FORMAT_MSG, sizeof(log_Format), \
@@ -2094,6 +2120,8 @@ struct PACKED log_Arm_Disarm {
       "MAV", "QBHHHBH",   "TimeUS,chan,txp,rxp,rxdp,flags,ss", "s#----s", "F-000-C" },   \
     { LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
       "VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
+    { LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
+      "VISP", "QQIffffff", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw", "sssmmmddh", "FFC000000" }, \
     { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \
       "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \
     { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \
@@ -2239,6 +2267,7 @@ enum LogMessages : uint8_t {
     LOG_RATE_MSG,
     LOG_RALLY_MSG,
     LOG_VISUALODOM_MSG,
+    LOG_VISUALPOS_MSG,
     LOG_AOA_SSA_MSG,
     LOG_BEACON_MSG,
     LOG_PROXIMITY_MSG,