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

offboard attitude sp: handle thrust only messages 

if attitude/rates haven been used previously do not set the ignore flags
even if the message asks us to do so to keep the controllers running
This commit is contained in:
Thomas Gubler 2015-02-16 21:47:11 +01:00
parent 3d29fa5f4e
commit 0389d30e0e
1 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -693,7 +693,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
mavlink_set_attitude_target_t set_attitude_target;
mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
struct offboard_control_mode_s offboard_control_mode;
static struct offboard_control_mode_s offboard_control_mode = {};
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
/* Only accept messages which are intended for this system */
@ -702,12 +702,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
(mavlink_system.compid == set_attitude_target.target_component ||
set_attitude_target.target_component == 0)) {
/* set correct ignore flags for body rate fields: copy from mavlink message */
offboard_control_mode.ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
/* set correct ignore flags for thrust field: copy from mavlink message */
offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
/* set correct ignore flags for attitude field: copy from mavlink message */
offboard_control_mode.ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
/*
* if attitude or body rate have been used (not ignored) previously and this message only sends
* throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
* body rates to keep the controllers running
*/
bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) {
/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate;
offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude;
} else {
offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
offboard_control_mode.ignore_attitude = ignore_attitude;
}
offboard_control_mode.timestamp = hrt_absolute_time();