yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: fix offboard velocity input

Browse Source 
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.
This commit is contained in:
Julian Oes
2021-07-21 10:54:29 +02:00
committed by Daniel Agar
parent 6b51c6390a
commit 349dd63072
1 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/mavlink/mavlink_receiver.cpp
View File

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