fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 20:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixing handling of attitude setpoints

Browse Source
This commit is contained in:
Andreas Antener
2015-03-15 12:10:53 +01:00
parent 030a96aa2c
commit 6f22cd07da
1 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/mavlink/mavlink.cpp
+8 -5
View File
@@ -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];
}