removed secondary attitude and log quaternion

3 files changed, 9 insertions, 52 deletions

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index e61204e6c..b0086676a 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -595,44 +595,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
-					// compute secondary attitude
-					math::Matrix<3, 3> R_adapted;		//modified rotation matrix
-					R_adapted = R;
-
-					//move z to x
-					R_adapted(0, 0) = R(0, 2);
-					R_adapted(1, 0) = R(1, 2);
-					R_adapted(2, 0) = R(2, 2);
-					//move x to z
-					R_adapted(0, 2) = R(0, 0);
-					R_adapted(1, 2) = R(1, 0);
-					R_adapted(2, 2) = R(2, 0);
-
-					//change direction of pitch (convert to right handed system)
-					R_adapted(0, 0) = -R_adapted(0, 0);
-					R_adapted(1, 0) = -R_adapted(1, 0);
-					R_adapted(2, 0) = -R_adapted(2, 0);
-					math::Vector<3> euler_angles_sec;		//adapted euler angles for fixed wing operation
-					euler_angles_sec = R_adapted.to_euler();
-
-					att.roll_sec    = euler_angles_sec(0);
-					att.pitch_sec   = euler_angles_sec(1);
-					att.yaw_sec     = euler_angles_sec(2);
-
-					memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec));
-
-					att.rollspeed_sec  = -x_aposteriori[2];
-					att.pitchspeed_sec = x_aposteriori[1];
-					att.yawspeed_sec   = x_aposteriori[0];
-					att.rollacc_sec    = -x_aposteriori[5];
-					att.pitchacc_sec   = x_aposteriori[4];
-					att.yawacc_sec     = x_aposteriori[3];
-
-					att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
-					att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
-					att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
-
-
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0cc193b6c..a3407e42c 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1437,6 +1437,10 @@ 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 = buf.att.roll;
 			log_msg.body.log_ATT.pitch = buf.att.pitch;
 			log_msg.body.log_ATT.yaw = buf.att.yaw;
@@ -1447,18 +1451,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_ATT.gy = buf.att.g_comp[1];
 			log_msg.body.log_ATT.gz = buf.att.g_comp[2];
 			LOGBUFFER_WRITE_AND_COUNT(ATT);
-			// secondary attitude
-			log_msg.msg_type = LOG_ATT2_MSG;
-			log_msg.body.log_ATT.roll = buf.att.roll_sec;
-			log_msg.body.log_ATT.pitch = buf.att.pitch_sec;
-			log_msg.body.log_ATT.yaw = buf.att.yaw_sec;
-			log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec;
-			log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec;
-			log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec;
-			log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0];
-			log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1];
-			log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2];
-			LOGBUFFER_WRITE_AND_COUNT(ATT);
 		}
 
 		/* --- ATTITUDE SETPOINT --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index c9221d58a..7080d9c31 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -50,8 +50,11 @@
 #pragma pack(push, 1)
 /* --- ATT - ATTITUDE --- */
 #define LOG_ATT_MSG 2
-#define LOG_ATT2_MSG 41
 struct log_ATT_s {
+	float q_w;
+	float q_x;
+	float q_y;
+	float q_z;
 	float roll;
 	float pitch;
 	float yaw;
@@ -460,7 +463,7 @@ struct log_PARM_s {
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
 	LOG_FORMAT_S(ATT, ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
-	LOG_FORMAT_S(ATT2, ATT, "fffffffff",	"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	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_S(IMU, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(IMU1, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),