Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 21 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 */ /* send out */
att.timestamp = raw.timestamp; att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0]; att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1]; att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2]; att.yawspeed = x_aposteriori[2];
att.rollacc = x_aposteriori[3]; // att.rollacc = x_aposteriori[3];
att.pitchacc = x_aposteriori[4]; // att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5]; // att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); // 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[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); // att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
/* copy offsets */ // /* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); // memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */ /* magnetic declination */
math::Matrix<3, 3> R_body = (&Rot_matrix[0]); matrix::Dcm<float> R(&Rot_matrix[0]);
R = R_decl * R_body; matrix::Dcm<float> R_declination(&R_decl.data[0][0]);
math::Quaternion q; R = R_declination * R;
q.from_dcm(R); matrix::Quaternion<float> q(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));
memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.q_valid = true;
att.R_valid = true;
if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {