[WIP] ekf2: initial motion capture (mocap) support with new modular structure

2026-11-10 02:31:36 +08:00 · 2023-08-18 19:50:13 -04:00
parent 068b1494fc
commit 0c1535f3e4
15 changed files with 307 additions and 3 deletions
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+
+#include "mocap.hpp"
+
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+
+void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
+{
+
+#if defined(MODULE_NAME)
+
+	if (_mocap_sub.updated()) {
+
+		vehicle_odometry_s mocap{};
+		_mocap_sub.copy(&mocap);
+
+		const int64_t time_us = mocap.timestamp_sample - static_cast<int64_t>(_param_ekf2_mocap_delay.get() * 1000);
+
+		MocapSample sample{};
+		sample.time_us = time_us;
+		sample.pos = Vector3f{mocap.position};
+		_mocap_buffer.push(sample);
+	}
+
+#endif // MODULE_NAME
+
+
+	MocapSample mocap_sample;
+
+	if (_mocap_buffer.pop_first_older_than(imu_delayed.time_us, &mocap_sample)) {
+
+		if (!ekf.global_origin_valid()) {
+			return;
+		}
+
+		// position
+		{
+			estimator_aid_source2d_s aid_src{};
+
+			const Vector2f pos_obs_var{
+				math::max(sq(mocap_sample.position_var(0)), sq(_param_ekf2_mocap_noise.get())),
+				math::max(sq(mocap_sample.position_var(1)), sq(_param_ekf2_mocap_noise.get()))
+			};
+
+			ekf.updateHorizontalPositionAidSrcStatus(mocap_sample.time_us,
+					mocap_sample.pos.xy(),                         // observation
+					pos_obs_var,                                   // observation variance
+					math::max(_param_ekf2_mocap_gate.get(), 1.f),  // innovation gate
+					aid_src);
+			aid_src.fusion_enabled = _param_ekf2_mocap_ctrl.get() & 0b001; // bit 0 Horizontal position
+
+			// TODO: check frame
+			if (ekf.control_status_flags().yaw_align) {
+				ekf.fuseHorizontalPosition(aid_src);
+			}
+
+#if defined(MODULE_NAME)
+			aid_src.timestamp = hrt_absolute_time();
+			_estimator_aid_src_mocap_xy_pub.publish(aid_src);
+#endif // MODULE_NAME
+		}
+
+	}
+}
+
+#endif // CONFIG_EKF2_MOCAP