fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 15:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove attitude setpoint matrix from attitude setpoint topic

Browse Source
This commit is contained in:
Lorenz Meier
2016-10-19 10:17:41 +02:00
committed by Lorenz Meier
parent 5317d29ffd
commit a12780c88f
8 changed files with 47 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+5 -1
View File
@@ -2584,7 +2584,11 @@ protected:
mavlink_attitude_target_t msg{};
msg.time_boot_ms = att_sp.timestamp / 1000;
mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
if (att_sp.q_d_valid) {
memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q));
} else {
mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
}
msg.body_roll_rate = att_rates_sp.roll;
msg.body_pitch_rate = att_rates_sp.pitch;
src/modules/mavlink/mavlink_receiver.cpp
-2
View File
@@ -1079,8 +1079,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
if (!ignore_attitude_msg) { // only copy att sp if message contained new data
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);
_att_sp.R_valid = true;
_att_sp.yaw_sp_move_rate = 0.0;
memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;