fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: update EV height state machine (small piece of EV overhaul)

Browse Source 
- respect new EKF2_EV_CTRL parameter for VPOS usage
  - EV hgt rotate EV position before usage (there's often a small offset in frames)
  - EV hgt reset use proper EV velocity body frame
  - try to keep EV hgt and EV vel state machines consistent
  - small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128
This commit is contained in:
Daniel Agar
2022-12-13 13:29:18 -05:00
committed by GitHub
parent 805ffa9d0b
commit 696eeb9a49
12 changed files with 149 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+5 -2
View File
@@ -400,7 +400,7 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the horizontal position observations
bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum);
bool starting_conditions_passing = quality_sufficient
const bool starting_conditions_passing = quality_sufficient
&& ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL)
&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
@@ -409,13 +409,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