From 9672dce3357cadfbfe2d14d9e51c1b229bd200f7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 15 Jul 2019 16:34:02 +1000
Subject: [PATCH] SITL: switch vicon simulation to use VISION_POSITION_ESTIMATE

this is a more commonly used message
---
 libraries/SITL/SIM_Vicon.cpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index f1b58d413c..a8775c354e 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -26,6 +26,9 @@
 
 using namespace SITL;
 
+#define USE_VISION_POSITION_ESTIMATE 1
+
+
 Vicon::Vicon()
 {
     int tmp[2];
@@ -104,6 +107,22 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
     float yaw;
     attitude.to_euler(roll, pitch, yaw);
 
+#if USE_VISION_POSITION_ESTIMATE
+    // use the more recent VISION_POSITION_ESTIMATE message
+    mavlink_msg_vision_position_estimate_pack_chan(
+        system_id,
+        component_id,
+        mavlink_ch,
+        &obs_msg,
+        now_us + time_offset_us,
+        position.x,
+        position.y,
+        position.z,
+        roll,
+        pitch,
+        yaw,
+        NULL, 0);
+#else
     mavlink_msg_vicon_position_estimate_pack_chan(
         system_id,
         component_id,
@@ -117,6 +136,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         pitch,
         yaw,
         NULL);
+#endif // USE_VISION_POSITION_ESTIMATE
 
     uint32_t delay_ms = 25 + unsigned(random()) % 300;
     time_send_us = now_us + delay_ms * 1000UL;