dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Bug in so3 estimator related to R matrix

This commit is contained in:
Hyon Lim (Retina) 2013-08-31 03:23:03 +09:00
parent 9f2419698f
commit 6a6fe69371
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
View File

@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// Because proper mount of PX4 will give you a reversed accelerometer readings. // Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
// Convert q->R. // Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13
@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
/* copy rotation matrix */ /* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); memcpy(&att.R, Rot_matrix, sizeof(float)*9);
att.R_valid = true; att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {