diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 65865c1616..021245bca4 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -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);