ekf2: EV overhaul yaw and position fusion (#20501)

- move EV yaw and EV position to new state machines - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK) - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion) - yaw_align now strictly means north (no more rotate external vision aid mask) - automatic switching between EV yaw, and yaw align north based on GPS quality
2026-07-14 06:00:35 +08:00 · 2022-12-20 10:23:56 -05:00
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ev_control.cpp
+ * Control functions for ekf external vision control
+ */
+
+#include "ekf.h"
+
+void Ekf::controlExternalVisionFusion()
+{
+	_ev_pos_b_est.predict(_dt_ekf_avg);
+
+	// Check for new external vision data
+	extVisionSample ev_sample;
+
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) {
+
+		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
+
+		// determine if we should use the horizontal position observations
+		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
+
+		const bool starting_conditions_passing = quality_sufficient
+				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
+				&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_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
+				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
+
+		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
+		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
+		controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
+
+		if (quality_sufficient) {
+			_ev_sample_prev = ev_sample;
+		}
+
+		// record corresponding yaw state for future EV delta heading innovation (logging only)
+		_ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
+
+	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw
+		    || _control_status.flags.ev_hgt)
+		   && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {
+
+		// Turn off EV fusion mode if no data has been received
+		stopEvPosFusion();
+		stopEvVelFusion();
+		stopEvYawFusion();
+		stopEvHgtFusion();
+
+		_warning_events.flags.vision_data_stopped = true;
+		ECL_WARN("vision data stopped");
+	}
+}