From 15ad9e441ef87b3e917a057e2f4b24d4bf3d0cee Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:38:39 +0100 Subject: compute secondary attitude with reference frame rotated -90 degress around pitch axis of original reference frame (used for VTOL) --- .../attitude_estimator_ekf_main.cpp | 38 ++++++++++++++++++++++ 1 file changed, 38 insertions(+) (limited to 'src/modules/attitude_estimator_ekf') 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 e2dbc30dd..6efcbc7f9 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -552,6 +552,44 @@ 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); -- cgit v1.2.3