From a530f16518333384295920e56cde636e39ba6809 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 2 Apr 2015 17:42:14 +0200 Subject: [PATCH] log quaternion setpoint --- src/modules/sdlog2/sdlog2.c | 4 ++++ src/modules/sdlog2/sdlog2_messages.h | 6 +++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1c8cdd4be0..0bfd356a1c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1495,6 +1495,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; + log_msg.body.log_ATSP.q_w = buf.att_sp.q_d[0]; + log_msg.body.log_ATSP.q_x = buf.att_sp.q_d[1]; + log_msg.body.log_ATSP.q_y = buf.att_sp.q_d[2]; + log_msg.body.log_ATSP.q_z = buf.att_sp.q_d[3]; LOGBUFFER_WRITE_AND_COUNT(ATSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 49483b15a2..a1fe2c95d0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,10 @@ struct log_ATSP_s { float pitch_sp; float yaw_sp; float thrust_sp; + float q_w; + float q_x; + float q_y; + float q_z; }; /* --- IMU - IMU SENSORS --- */ @@ -473,7 +477,7 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), + LOG_FORMAT(ATSP, "ffffffff", "RollSP,PitchSP,YawSP,ThrustSP,qw,qx,qy,qz"), LOG_FORMAT_S(IMU, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(IMU1, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(IMU2, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"),