From b46e505b0dc0dd324f16d9fe641b84f54ee9857e Mon Sep 17 00:00:00 2001
From: alessandro <3762382+potaito@users.noreply.github.com>
Date: Tue, 6 Apr 2021 14:59:56 +0200
Subject: [PATCH] Handle velocity fields in follow_target message

The mavlink message FOLLOW_TARGET has additional
fields that are not being parsed here. See
https://mavlink.io/en/messages/common.html#FOLLOW_TARGET
for more details.
---
 src/modules/mavlink/mavlink_receiver.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7a8d5f545b..7035a6622e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2284,6 +2284,9 @@ MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
 	follow_target_topic.lat = follow_target_msg.lat * 1e-7;
 	follow_target_topic.lon = follow_target_msg.lon * 1e-7;
 	follow_target_topic.alt = follow_target_msg.alt;
+	follow_target_topic.vx = follow_target_msg.vel[0];
+	follow_target_topic.vy = follow_target_msg.vel[1];
+	follow_target_topic.vz = follow_target_msg.vel[2];
 
 	_follow_target_pub.publish(follow_target_topic);
 }