Unverified Commit 349dd630 authored by Julian Oes's avatar Julian Oes Committed by Daniel Agar
Browse files

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.
parent 6b51c639
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment