fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 03:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: update EV height state machine

Browse Source 
- respect new EKF2_EV_CTRL parameter for HPOS usage
  - EV hgt rotate EV position before usage (there's often a small offset in frames)
  - EV height reset use proper EV velocity body frame
  - try to keep EV hgt and EV vel state machines consistent
This commit is contained in:
Daniel Agar
2022-10-28 10:15:03 -04:00
parent 897e9f0593
commit 7750d610f1
10 changed files with 144 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+4 -1
View File
@@ -387,13 +387,16 @@ void Ekf::controlExternalVisionFusion()
// EV velocity
controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
// EV height
controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
// record observation and estimate for use next time
_ev_sample_delayed_prev = _ev_sample_delayed;
_hpos_pred_prev = _state.pos.xy();
_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw)
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt)
&& !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received