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

@@ -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;