From e01eaf172a50727ba241513303df0212b83e3558 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Nov 2016 15:53:39 +0100 Subject: [PATCH] Add body/NED frame for offboard velocity control 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. --- msg/position_setpoint.msg | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 2 ++ .../mc_pos_control/mc_pos_control_main.cpp | 20 +++++++++++++++---- 3 files changed, 23 insertions(+), 4 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index fa3b40389d..c3ff1c095b 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -9,6 +9,9 @@ uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard uint8 SETPOINT_TYPE_FOLLOW_TARGET=7 # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target +uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED +uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED + bool valid # true if setpoint is valid uint8 type # setpoint type to adjust behavior of position controller float32 x # local position setpoint in m in NED @@ -19,6 +22,8 @@ float32 vx # local velocity setpoint in m/s in NED float32 vy # local velocity setpoint in m/s in NED float32 vz # local velocity setpoint in m/s in NED bool velocity_valid # true if local velocity setpoint valid +uint8 velocity_frame # to set velocity setpoints in NED or body + bool alt_valid # do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control. float64 lat # latitude, in deg float64 lon # longitude, in deg diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c5636f70e0..5514bd5421 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index daef95f8f5..b71a1bbf85 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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;