From eb18622d85a7dbd238ffeef78091e11b33146584 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 20 Apr 2016 22:19:24 +0200 Subject: [PATCH] added old ekf attitude estimator back to config and made changes so it compiles --- .../attitude_estimator_ekf_main.cpp | 34 ++++++++----------- 1 file changed, 15 insertions(+), 19 deletions(-) diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 480c4cdd21..cf283cba5d 100755 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -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 R(&Rot_matrix[0]); + matrix::Dcm R_declination(&R_decl.data[0][0]); + R = R_declination * R; + matrix::Quaternion 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])) {