1 files changed, 42 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index e608bf787..58af1fcb6 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -641,6 +641,47 @@ protected:
 };
 
 
+
+class MavlinkStreamViconPositionEstimate : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "VICON_POSITION_ESTIMATE";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamViconPositionEstimate();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	struct vehicle_vicon_position_s *pos;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
+		pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (pos_sub->update(t)) {
+			mavlink_msg_vicon_position_estimate_send(_channel,
+								pos->timestamp / 1000,
+								pos->x,
+								pos->y,
+								pos->z,
+								pos->roll,
+								pos->pitch,
+								pos->yaw);
+		}
+	}
+};
+
+
 class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
 {
 public:
@@ -1344,5 +1385,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
 	new MavlinkStreamDistanceSensor(),
+	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };