Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2_replay: compute euler angles for logging

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-09 20:04:44 +02:00
committed by Lorenz Meier
parent 17a4b64434
commit 7a7bf2205d
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2_replay/ekf2_replay_main.cpp
+5 -6
View File
@@ -535,15 +535,14 @@ void Ekf2Replay::logIfUpdated()
log_message.body.att.q_x = att.q[1];
log_message.body.att.q_y = att.q[2];
log_message.body.att.q_z = att.q[3];
log_message.body.att.roll = 0;
log_message.body.att.pitch = 0;
log_message.body.att.yaw = 0;
log_message.body.att.roll = atan2f(2 * (att.q[0] * att.q[1] + att.q[2] * att.q[3]),
1 - 2 * (att.q[1] * att.q[1] + att.q[2] * att.q[2]));
log_message.body.att.pitch = asinf(2 * (att.q[0] * att.q[2] - att.q[3] * att.q[1]));
log_message.body.att.yaw = atan2f(2 * (att.q[0] * att.q[3] + att.q[1] * att.q[2]),
1 - 2 * (att.q[2] * att.q[2] + att.q[3] * att.q[3]));
log_message.body.att.roll_rate = att.rollspeed;
log_message.body.att.pitch_rate = att.pitchspeed;
log_message.body.att.yaw_rate = att.yawspeed;
log_message.body.att.gx = 0;
log_message.body.att.gy = 0;
log_message.body.att.gz = 0;
writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length);