From 19c4fdd7c5c905dfe4e0791b7d6accb4bb036c57 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 31 Mar 2021 23:33:30 +0200 Subject: [PATCH] Add support for 3D body thrust setpoint for offboard attitude control This adds support for filling up the 3D thrust setpoint using SET_ATTITUDE_TARGET message Update mavlink submodule --- mavlink/include/mavlink/v2.0 | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 9b429ea239..9e07c7d0b6 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 9b429ea239d7230d7f0329cfbec4a817162670ea +Subproject commit 9e07c7d0b6eb91f0fd84f911dc91e68e865ba7ed diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 950dc707d0..29a2919687 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1418,6 +1418,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) const bool attitude = !(type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE); const bool body_rates = !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) && !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE); + const bool thrust_body = (type_mask & ATTITUDE_TARGET_TYPEMASK_THRUST_BODY_SET); vehicle_status_s vehicle_status{}; _vehicle_status_sub.copy(&vehicle_status); @@ -1437,8 +1438,13 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? NAN : attitude_target.body_yaw_rate; - if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { + if (!thrust_body && !(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust); + + } else if (thrust_body) { + attitude_setpoint.thrust_body[0] = attitude_target.thrust_body[0]; + attitude_setpoint.thrust_body[1] = attitude_target.thrust_body[1]; + attitude_setpoint.thrust_body[2] = attitude_target.thrust_body[2]; } // publish offboard_control_mode