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

attitude message cleanup: more cleanup

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-18 20:46:08 +02:00
committed by Lorenz Meier
parent 3faaeb06d1
commit a978d61d9a
6 changed files with 20 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+6 -8
View File
@@ -841,12 +841,11 @@ protected:
if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_t msg;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
matrix::Eulerf euler = matrix::Quatf(att.q);
msg.time_boot_ms = att.timestamp / 1000;
msg.roll = euler(0);
msg.pitch = euler(1);
msg.yaw = euler(2);
msg.roll = euler.phi();
msg.pitch = euler.theta();
msg.yaw = euler.psi();
msg.rollspeed = att.rollspeed;
msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed;
@@ -1015,11 +1014,10 @@ protected:
if (updated) {
mavlink_vfr_hud_t msg;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
matrix::Eulerf euler = matrix::Quatf(att.q);
msg.airspeed = airspeed.indicated_airspeed_m_s;
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(euler(2)) * M_RAD_TO_DEG_F;
msg.heading = _wrap_2pi(euler.psi()) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
if (_pos_time > 0) {