fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 17:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add body/NED frame for offboard velocity control

Browse Source 
This adds the possibility to use offboard velocity control in the body
frame and not just the NED (world) frame.

The frame is set in the set_position_target_local_ned message and passed
on to mc_pos_control in the position_setpoint topic.
This commit is contained in:
Julian Oes
2016-12-15 10:17:03 +01:00
committed by Lorenz Meier
parent e3aecc0762
commit e01eaf172a
3 changed files with 23 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+2
View File
@@ -868,6 +868,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
pos_sp_triplet.current.velocity_frame =
set_position_target_local_ned.coordinate_frame;
} else {
pos_sp_triplet.current.velocity_valid = false;
}
src/modules/mc_pos_control/mc_pos_control_main.cpp
+16 -4
View File
@@ -1222,12 +1222,24 @@ MulticopterPositionControl::control_offboard(float dt)
_hold_offboard_xy = true;
}
_run_pos_control = false;
_run_pos_control = true;
} else {
/* set position setpoint move rate */
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
/* set position setpoint move rate */
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
} else if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
// Transform velocity command from body frame to NED frame
_vel_sp(0) = cosf(_yaw) * _pos_sp_triplet.current.vx - sinf(_yaw) * _pos_sp_triplet.current.vy;
_vel_sp(1) = sinf(_yaw) * _pos_sp_triplet.current.vx + cosf(_yaw) * _pos_sp_triplet.current.vy;
} else {
PX4_WARN("Unknown velocity offboard coordinate frame");
}
_run_pos_control = false;
_hold_offboard_xy = false;