From 11480cdbc7cee0ee0c05098555a912d1dfa5d357 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Jul 2021 10:54:29 +0200 Subject: [PATCH] mavlink: fix offboard velocity input This reverts the behavior for offboard velocity setpoint. Back in v1.11, the velocity input in NED_BODY was assumed to be in the world frame but rotated by yaw to the vehicle frame. With the current state the frame is completely fixed to the body. While this might be technically correct, it doesn't seem much intuitive for multicopters and breaks the MAVSDK offboard velocity API. So as an example, with a velocity setpoint of 5 m/s forward, the drone would in - v1.11: fly forward with 5 m/s - v1.12: start to fly forward by pitching down but then descend rapidly as the forward velocity would translate to a setpoint in Z into the ground as it is pitched down. This commit restores the behavior to what we had previously. --- src/modules/mavlink/mavlink_receiver.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7324530d49..838d05c665 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -920,10 +920,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? 0.f : target_local_ned.vz }; - const matrix::Vector3f velocity_setpoint{R * velocity_body_sp}; - setpoint.vx = velocity_setpoint(0); - setpoint.vy = velocity_setpoint(1); - setpoint.vz = velocity_setpoint(2); + + const float yaw = matrix::Eulerf{R}(2); + + setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1); + setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1); + setpoint.vz = velocity_body_sp(2); } else { setpoint.vx = NAN;