diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-27 14:42:30 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-27 14:42:30 +0400 |
commit | fc0ffbbd63036d094ad61a328e406c64742b481d (patch) | |
tree | c9407998005a7f5bbaa2f0c0e7d327163c86de74 /src/modules | |
parent | 6a04d13e733700e7e2e90c20399889a79eae293a (diff) | |
parent | 464df9c5e8cac88c24ee080337970df74edcd239 (diff) | |
download | px4-firmware-fc0ffbbd63036d094ad61a328e406c64742b481d.tar.gz px4-firmware-fc0ffbbd63036d094ad61a328e406c64742b481d.tar.bz2 px4-firmware-fc0ffbbd63036d094ad61a328e406c64742b481d.zip |
Merge branch 'ekf_acc_comp' into vector_control2
Diffstat (limited to 'src/modules')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 18 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 2 |
2 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 db0ef6228..36e82db43 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -276,6 +276,9 @@ 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(); @@ -371,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; @@ -388,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; @@ -481,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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b4f7f2dfe..f8694ef28 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -579,6 +579,7 @@ handle_message(mavlink_message_t *msg) hil_gps.alt = gps.alt; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.timestamp_variance = gps.time_usec; hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -590,6 +591,7 @@ handle_message(mavlink_message_t *msg) if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m |