diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2015-01-19 10:19:18 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-20 13:01:41 +0100 |
commit | 59cb0cc3b4d91b4dd56369c47d88b4876e9784a9 (patch) | |
tree | 561ffd1b2211a6a0f20729a9bdb8881c08ac0a5c /src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | |
parent | 70c7764b9f9e780664ae13d0962a55dd226117e6 (diff) | |
download | px4-firmware-59cb0cc3b4d91b4dd56369c47d88b4876e9784a9.tar.gz px4-firmware-59cb0cc3b4d91b4dd56369c47d88b4876e9784a9.tar.bz2 px4-firmware-59cb0cc3b4d91b4dd56369c47d88b4876e9784a9.zip |
removed secondary attitude and log quaternion
Diffstat (limited to 'src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 38 |
1 files changed, 0 insertions, 38 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); |