fixing handling of attitude setpoints

RomanBapst · Mar 15, 2015 · 6f22cd0 · 6f22cd0
1 parent 030a96a
commit 6f22cd0
Showing 1 changed file with 8 additions and 5 deletions.
diff --git 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];
 		}