diff options
| author | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-08-31 03:23:03 +0900 |
|---|---|---|
| committer | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-08-31 03:23:03 +0900 |
| commit | 6a6fe6937146723cb86dc0fc6e46db38b328dacd (patch) | |
| tree | c0bded8527d487c8a324eeb62ac7d40001a7eb4c /src | |
| parent | 9f2419698ff4d0703f58e67f29f4981fa7b4f0c5 (diff) | |
| download | px4-firmware-6a6fe6937146723cb86dc0fc6e46db38b328dacd.tar.gz px4-firmware-6a6fe6937146723cb86dc0fc6e46db38b328dacd.tar.bz2 px4-firmware-6a6fe6937146723cb86dc0fc6e46db38b328dacd.zip | |
Bug in so3 estimator related to R matrix
Diffstat (limited to 'src')
| -rwxr-xr-x | src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 236052b56..86bda3c75 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -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. 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[1] = 2.0 * (q1*q2 + q0*q3); // 12 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)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { |