diff options
| -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 |