log quaternion setpoint

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);
 		}
 

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"),