fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 04:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

added old ekf attitude estimator back to config and made changes so it compiles

Browse Source
This commit is contained in:
Roman
2016-04-20 22:19:24 +02:00
committed by Lorenz Meier
parent b8a219d351
commit eb18622d85
1 changed files with 15 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+15 -19
View File
@@ -605,34 +605,30 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
att.rollacc = x_aposteriori[3];
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
// att.rollacc = x_aposteriori[3];
// att.pitchacc = x_aposteriori[4];
// att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
// att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
// att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
// att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
// /* copy offsets */
// memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
R = R_decl * R_body;
math::Quaternion q;
q.from_dcm(R);
/* copy rotation matrix */
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
memcpy(&att.q[0],&q.data[0],sizeof(att.q));
att.R_valid = true;
matrix::Dcm<float> R(&Rot_matrix[0]);
matrix::Dcm<float> R_declination(&R_decl.data[0][0]);
R = R_declination * R;
matrix::Quaternion<float> q(R);
memcpy(&att.q[0],&q._data[0],sizeof(att.q));
att.q_valid = true;
if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {