fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

removed secondary attitude and log quaternion

Browse Source
This commit is contained in:
Roman Bapst
2015-01-20 13:01:41 +01:00
committed by Lorenz Meier
parent 70c7764b9f
commit 59cb0cc3b4
3 changed files with 9 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
-38
View File
@@ -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);
src/modules/sdlog2/sdlog2.c
+4 -12
View File
@@ -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 --- */
src/modules/sdlog2/sdlog2_messages.h
+5 -2
View File
@@ -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"),