From 8b0ec5a78e03514ade6f4ddd231f4db37f8ee500 Mon Sep 17 00:00:00 2001 From: Benoit Landry Date: Mon, 1 Feb 2021 18:31:36 -0800 Subject: [PATCH] mavlink: ATTITUDE_TARGET thrust use thrust_body magnitude - vehicle_attitude_setpoint thrust_body is a vector, but mavlink ATTITUDE_TARGET thrust is only a magnitude - this allows the stream to be correct for both MC & FW use cases --- src/modules/mavlink/streams/ATTITUDE_TARGET.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp index 02f77bf8bb..5b7886d5ab 100644 --- a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp @@ -100,7 +100,7 @@ protected: msg.body_pitch_rate = att_rates_sp.pitch; msg.body_yaw_rate = att_rates_sp.yaw; - msg.thrust = att_sp.thrust_body[0]; + msg.thrust = Vector3f(att_sp.thrust_body).norm(); mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);