diff options
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 | 42 |
1 files changed, 27 insertions, 15 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 efa6d97fd..36e82db43 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -62,6 +62,8 @@ #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> +#include <lib/mathlib/mathlib.h> + #include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -274,6 +276,13 @@ 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; + /* actual acceleration (by GPS velocity) in body frame */ + float acc[3] = { 0.0f, 0.0f, 0.0f }; + + /* 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"); @@ -308,6 +317,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 */ @@ -362,7 +374,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - float acc[3]; if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { if (last_gps != 0 && gps.timestamp_velocity != last_gps) { float gps_dt = (gps.timestamp_velocity - last_gps) / 1000000.0f; @@ -379,20 +390,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds acc[i] += att.R[j][i] * acc_NED[j]; } } + + vel_prev[0] = gps.vel_n_m_s; + vel_prev[1] = gps.vel_e_m_s; + vel_prev[2] = gps.vel_d_m_s; } + last_gps = gps.timestamp_velocity; } else { acc[0] = 0.0f; acc[1] = 0.0f; acc[2] = 0.0f; - } - if (gps.vel_ned_valid) { - vel_prev[0] = gps.vel_n_m_s; - vel_prev[1] = gps.vel_e_m_s; - vel_prev[2] = gps.vel_d_m_s; - last_gps = gps.timestamp_velocity; - } else { vel_prev[0] = 0.0f; vel_prev[1] = 0.0f; vel_prev[2] = 0.0f; @@ -472,7 +481,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) + if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; @@ -480,10 +489,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -492,12 +500,16 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - //att.yawspeed =z_k[2] ; /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + /* magnetic declination */ + + math::Matrix<3, 3> R_body = (&Rot_matrix[0]); + math::Matrix<3, 3> R = R_decl * R_body; + /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + 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)) { |