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

Adds reset counter logic to EV height fusion

Browse Source
This commit is contained in:
Charles Cross
2022-09-26 11:48:18 +02:00
committed by Mathieu Bresciani
parent 779e738143
commit cfdaba35cc
3 changed files with 20 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ev_height_control.cpp
+11 -4
View File
@@ -50,17 +50,24 @@ void Ekf::controlEvHeightFusion()
const bool ev_intermittent = !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
if (_ev_data_ready) {
const bool continuing_conditions_passing = !ev_intermittent;
const bool position_valid = PX4_ISFINITE(_ev_sample_delayed.pos(2));
const bool continuing_conditions_passing = !ev_intermittent && position_valid;
const bool starting_conditions_passing = continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
if (continuing_conditions_passing) {
fuseEvHgt();
if (isHeightResetRequired()) {
// All height sources are failing
const bool reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
if (isHeightResetRequired() || reset ) {
resetHeightToEv();
resetVerticalVelocityToZero();
// If the sample has a valid vertical velocity estimate, use it
if (PX4_ISFINITE(_ev_sample_delayed.vel(2))) {
resetVerticalVelocityToEv(_ev_sample_delayed);
} else {
resetVerticalVelocityToZero();
}
}
} else {