dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Jaeyoung-Lim 2021-03-31 23:33:30 +02:00 committed by Lorenz Meier
parent 8d3335906a
commit 19c4fdd7c5
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mavlink/include/mavlink/v2.0

@ -1 +1 @@
Subproject commit 9b429ea239d7230d7f0329cfbec4a817162670ea
Subproject commit 9e07c7d0b6eb91f0fd84f911dc91e68e865ba7ed

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

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