fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 12:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: accumulate multiple vel/pos/orientation reset deltas per update

Browse Source 
- if in a single EKF update there are multiple resets we need to track the accumulated delta so the change consumed by the controllers is correct
This commit is contained in:
Daniel Agar
2022-11-15 13:20:54 -05:00
committed by GitHub
parent 2e918eba00
commit da82757bf6
4 changed files with 95 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+5 -5
View File
@@ -1340,11 +1340,11 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
_ekf.get_ekf_soln_status(&status.solution_status_flags);
// reset counters
status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
status.reset_count_vel_d = _ekf.state_reset_status().velD_counter;
status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter;
status.reset_count_pod_d = _ekf.state_reset_status().posD_counter;
status.reset_count_quat = _ekf.state_reset_status().quat_counter;
status.reset_count_vel_ne = _ekf.state_reset_status().reset_count.velNE;
status.reset_count_vel_d = _ekf.state_reset_status().reset_count.velD;
status.reset_count_pos_ne = _ekf.state_reset_status().reset_count.posNE;
status.reset_count_pod_d = _ekf.state_reset_status().reset_count.posD;
status.reset_count_quat = _ekf.state_reset_status().reset_count.quat;
status.time_slip = _last_time_slip_us * 1e-6f;