From 349dd63072e6d4d79c0a039660051043e30a824c 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 2267467b9e..adeeb8fa3a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -911,10 +911,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;