diff options
| author | Andreas Antener <antener_a@gmx.ch> | 2015-03-15 12:10:53 +0100 |
|---|---|---|
| committer | Andreas Antener <antener_a@gmx.ch> | 2015-03-15 12:10:53 +0100 |
| commit | 6f22cd07da4bf9295fb18abab43589ad790cb4a8 (patch) | |
| tree | 73ab740ef4d51374196106ca33f6f79f728bdf25 /src/platforms | |
| parent | 030a96aa2c4a512a71b32dbd616eb8411ac67c6e (diff) | |
| download | px4-firmware-6f22cd07da4bf9295fb18abab43589ad790cb4a8.tar.gz px4-firmware-6f22cd07da4bf9295fb18abab43589ad790cb4a8.tar.bz2 px4-firmware-6f22cd07da4bf9295fb18abab43589ad790cb4a8.zip | |
fixing handling of attitude setpoints
Diffstat (limited to 'src/platforms')
| -rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.cpp | 13 |
1 files changed, 8 insertions, 5 deletions
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 5459fcffd..c8188f937 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -162,16 +162,19 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ att_sp.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, - &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); - att_sp.R_valid = true; + if (!ignore_attitude) { + mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, + &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); + att_sp.R_valid = true; + } + if (!offboard_control_mode.ignore_thrust) { att_sp.thrust = set_attitude_target.thrust; } - if (!offboard_control_mode.ignore_attitude) { + if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { att_sp.q_d[i] = set_attitude_target.q[i]; } |