diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-16 21:47:11 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 15:19:15 +0100 |
commit | 0389d30e0ee07b7d3d81da99dacfc1d5854eed26 (patch) | |
tree | 33ccbcc7ce9e944936740941796b611e8099a803 /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 3d29fa5f4e387457ad425425a70c3a0792f808e7 (diff) | |
download | px4-firmware-0389d30e0ee07b7d3d81da99dacfc1d5854eed26.tar.gz px4-firmware-0389d30e0ee07b7d3d81da99dacfc1d5854eed26.tar.bz2 px4-firmware-0389d30e0ee07b7d3d81da99dacfc1d5854eed26.zip |
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
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 23 |
1 files changed, 18 insertions, 5 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3e09e5c81..abaec51e4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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(); |