diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-25 18:41:23 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-25 18:41:23 +0400 |
commit | 26daae0b0af2f73171f2e741178474873724fdbe (patch) | |
tree | 82ef7934249fa285d7ea32d0d839812641c0a6a1 /src/modules/attitude_estimator_ekf | |
parent | 1e4bb764a61335c0e209f83438e74264e972a164 (diff) | |
parent | 69218d2bd58a12a2ce89066a5e6a9e8682529cd5 (diff) | |
download | px4-firmware-26daae0b0af2f73171f2e741178474873724fdbe.tar.gz px4-firmware-26daae0b0af2f73171f2e741178474873724fdbe.tar.bz2 px4-firmware-26daae0b0af2f73171f2e741178474873724fdbe.zip |
Merge branch 'mathlib_new' into vector_control2
Diffstat (limited to 'src/modules/attitude_estimator_ekf')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 20 |
1 files changed, 11 insertions, 9 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 983ac7c8d..48a11e7fc 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -267,6 +267,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* rotation matrix for magnetic declination */ + math::Matrix<3, 3> R_decl; + R_decl.identity(); + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -301,6 +305,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -450,17 +457,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ - math::EulerAngles eulerMagDecl(0.0f, 0.0f, ekf_params.mag_decl); - math::Dcm R(eulerMagDecl); - math::Dcm R_body(&Rot_matrix[0]); - R = R * R_body; + + math::Matrix<3, 3> R_body = (&Rot_matrix[0]); + math::Matrix<3, 3> R = R_decl * R_body; /* copy rotation matrix */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - att.R[i][j] = R(i, j); - } - } + memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { |