Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-27 08:54:31 +08:00
Code Issues Actions 40 Packages Projects Releases Wiki Activity

ros: mavlink node: update to latest offboard code

Browse Source
This commit is contained in:
Thomas Gubler
2015-02-28 18:25:35 +01:00
parent 03e900d3f1
commit 80fbb512c9
1 changed files with 25 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/mavlink/mavlink.cpp
+25 -11
View File
@@ -124,16 +124,30 @@ void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t c
void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg)
{ {
mavlink_set_attitude_target_t set_att_target; mavlink_set_attitude_target_t set_attitude_target;
mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target);
offboard_control_mode offboard_control_mode; offboard_control_mode offboard_control_mode;
/* set correct ignore flags for body rate fields: copy from mavlink message */
offboard_control_mode.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7);
/* set correct ignore flags for thrust field: copy from mavlink message */ /* set correct ignore flags for thrust field: copy from mavlink message */
offboard_control_mode.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); 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_att_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 = get_time_micros(); offboard_control_mode.timestamp = get_time_micros();
_offboard_control_mode_pub.publish(offboard_control_mode); _offboard_control_mode_pub.publish(offboard_control_mode);
@@ -145,13 +159,13 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg)
*/ */
att_sp.timestamp = get_time_micros(); att_sp.timestamp = get_time_micros();
mavlink_quaternion_to_euler(set_att_target.q, &att_sp.roll_body, &att_sp.pitch_body, mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body,
&att_sp.yaw_body); &att_sp.yaw_body);
mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])att_sp.R_body.data()); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data());
att_sp.R_valid = true; att_sp.R_valid = true;
att_sp.thrust = set_att_target.thrust; att_sp.thrust = set_attitude_target.thrust;
for (ssize_t i = 0; i < 4; i++) { for (ssize_t i = 0; i < 4; i++) {
att_sp.q_d[i] = set_att_target.q[i]; att_sp.q_d[i] = set_attitude_target.q[i];
} }
att_sp.q_d_valid = true; att_sp.q_d_valid = true;