yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2141 from PX4/quat_sp

Browse Source 
copy quaternion setpoint into attitude setpoint topic
This commit is contained in:
Lorenz Meier
2015-05-07 14:29:26 +02:00
parent ea8ba79481 5cf8efcc60
commit 5003875911
1 changed files with 15 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@@ -1285,6 +1285,11 @@ MulticopterPositionControl::task_main()
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* copy quaternion setpoint to attitude setpoint topic */
math::Quaternion q_sp;
q_sp.from_dcm(R);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
@@ -1300,6 +1305,11 @@ MulticopterPositionControl::task_main()
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* copy quaternion setpoint to attitude setpoint topic */
math::Quaternion q_sp;
q_sp.from_dcm(R);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
@@ -1385,6 +1395,11 @@ MulticopterPositionControl::task_main()
math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
/* copy quaternion setpoint to attitude setpoint topic */
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
_att_sp.timestamp = hrt_absolute_time();
}
else {