zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

calculate euler angles for logging

sbg
tumbili 9 years ago committed by Lorenz Meier
parent
0d0fa133e6
commit
38b949a5aa
1 changed files with 11 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      src/modules/sdlog2/sdlog2.c

18
src/modules/sdlog2/sdlog2.c
Unescape View File

@ -2265,13 +2265,17 @@ int sdlog2_thread_main(int argc, char *argv[]) @@ -2265,13 +2265,17 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- ATTITUDE --- */
if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) {
log_msg.msg_type = LOG_ATT_MSG;
log_msg.body.log_ATT.q_w = buf.att.q[0];
log_msg.body.log_ATT.q_x = buf.att.q[1];
log_msg.body.log_ATT.q_y = buf.att.q[2];
log_msg.body.log_ATT.q_z = buf.att.q[3];
log_msg.body.log_ATT.roll = 0;
log_msg.body.log_ATT.pitch = 0;
log_msg.body.log_ATT.yaw = 0;
float q0 = buf.att.q[0];
float q1 = buf.att.q[1];
float q2 = buf.att.q[2];
float q3 = buf.att.q[3];
log_msg.body.log_ATT.q_w = q0;
log_msg.body.log_ATT.q_x = q1;
log_msg.body.log_ATT.q_y = q2;
log_msg.body.log_ATT.q_z = q3;
log_msg.body.log_ATT.roll = atan2f(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
log_msg.body.log_ATT.pitch = asinf(2*(q0*q2 - q3*q1));
log_msg.body.log_ATT.yaw = atan2f(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;

Write Preview
Loading…
Cancel
Save