hacks

.vscode/settings.json

@@ -127,5 +127,6 @@
         "terminal.integrated.scrollback": 15000,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        }
+        },
+        "ros.distro": "humble"
 }

boards/px4/sitl/default.px4board

@@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+CONFIG_EKF2_MOCAP=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y

msg/EstimatorAidSource1d.msg

@@ -21,4 +21,5 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
+# TOPICS estimator_aid_src_mocap_z estimator_aid_src_mocap_yaw
 # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw

msg/EstimatorAidSource2d.msg

@@ -19,4 +19,4 @@ bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_mocap_xy estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow

msg/EstimatorStatusFlags.msg

@@ -42,6 +42,7 @@ bool cs_gravity_vector          # 34 - true when gravity vector measurements are
 bool cs_mag                     # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
 bool cs_ev_yaw_fault            # 36 - true when the EV heading has been declared faulty and is no longer being used
 bool cs_mag_heading_consistent  # 37 - true when the heading obtained from mag data is declared consistent with the filter
+bool cs_mocap                   # 38 - true when motion capture (mocap) fusion is intended
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/CMakeLists.txt

@@ -99,6 +99,10 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MOCAP)
+	list(APPEND EKF_SRCS EKF/mocap.cpp)
+endif()
+
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -64,6 +64,10 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS airspeed_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MOCAP)
+	list(APPEND EKF_SRCS mocap.cpp)
+endif()
+
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/common.h

@@ -113,11 +113,12 @@ enum TerrainFusionMask : uint8_t {
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 enum HeightSensor : uint8_t {
-	BARO  = 0,
-	GNSS  = 1,
-	RANGE = 2,
-	EV    = 3,
-	UNKNOWN  = 4
+	BARO    = 0,
+	GNSS    = 1,
+	RANGE   = 2,
+	EV      = 3,
+	MOCAP   = 4,
+	UNKNOWN = 5
 };
 
 enum class PositionSensor : uint8_t {
@@ -601,7 +602,7 @@ union filter_control_status_u {
 		uint64_t mag                     : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
 		uint64_t ev_yaw_fault            : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
 		uint64_t mag_heading_consistent  : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
-
+		uint64_t mocap                   : 1; ///< 38 - true when motion capture (mocap) fusion is intended
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -111,6 +111,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 	controlGpsFusion(imu_delayed);
 
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+	_mocap.update(*this, imu_delayed);
+#endif // CONFIG_EKF2_MOCAP
+
 #if defined(CONFIG_EKF2_AIRSPEED)
 	controlAirDataFusion(imu_delayed);
 #endif // CONFIG_EKF2_AIRSPEED

src/modules/ekf2/EKF/ekf.cpp

@@ -316,3 +316,10 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	// Note fixed coefficients are used to save operations. The exact time constant is not important.
 	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
 }
+
+void Ekf::updateParameters()
+{
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+	_mocap.updateParameters();
+#endif // CONFIG_EKF2_MOCAP
+}

src/modules/ekf2/EKF/ekf.h

@@ -54,6 +54,10 @@
 #include <uORB/topics/estimator_aid_source2d.h>
 #include <uORB/topics/estimator_aid_source3d.h>
 
+#if defined(CONFIG_EKF2_MOCAP)
+# include "mocap.hpp"
+#endif // CONFIG_EKF2_MOCAP
+
 enum class Likelihood { LOW, MEDIUM, HIGH };
 
 class Ekf final : public EstimatorInterface
@@ -266,6 +270,8 @@ public:
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
 
+	auto& getState() const { return _state; }
+
 	// get the wind velocity var
 	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
 
@@ -506,6 +512,10 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
+	void updateParameters();
+
+	friend class Mocap;
+
 private:
 
 	// set the internal states and status to their default value
@@ -1242,6 +1252,10 @@ private:
 		// if any of the innovations are rejected, then the overall innovation is rejected
 		status.innovation_rejected = innovation_rejected;
 	}
+
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+	Mocap _mocap{};
+#endif // CONFIG_EKF2_MOCAP
 };
 
 #endif // !EKF_EKF_H

src/modules/ekf2/EKF/estimator_interface.h

@@ -270,6 +270,10 @@ public:
 	const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
 	const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
 
+	// TODO
+	void enableControlStatusMocap() { _control_status.flags.mocap = true; }
+	void disableControlStatusMocap() { _control_status.flags.mocap = false; }
+
 	// get EKF internal fault status
 	const fault_status_u &fault_status() const { return _fault_status; }
 	const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -69,7 +69,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 			pos = ev_sample.pos - pos_offset_earth;
 			pos_cov = matrix::diag(ev_sample.position_var);
 
-			if (_control_status.flags.gps) {
+			if (_control_status.flags.gps || _control_status.flags.mocap) {
 				_ev_pos_b_est.setFusionActive();
 
 			} else {
@@ -108,7 +108,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 				pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
 			}
 
-			if (_control_status.flags.gps) {
+			if (_control_status.flags.gps || _control_status.flags.mocap) {
 				_ev_pos_b_est.setFusionActive();
 
 			} else {
@@ -125,13 +125,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 		break;
 	}
 
-	// increase minimum variance if GPS active (position reference)
-	if (_control_status.flags.gps) {
-		for (int i = 0; i < 2; i++) {
-			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
-		}
-	}
-
 	const Vector2f measurement{pos(0), pos(1)};
 
 	const Vector2f measurement_var{
@@ -200,7 +193,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
 {
 	// activate fusion
 	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
-	if (_control_status.flags.gps) {
+	if (_control_status.flags.gps || _control_status.flags.mocap) {
 		ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
 		_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
 		_ev_pos_b_est.setFusionActive();
@@ -226,7 +219,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 
 		if (quality_sufficient) {
 
-			if (!_control_status.flags.gps) {
+			if (!_control_status.flags.gps && !_control_status.flags.mocap) {
 				ECL_INFO("reset to %s", EV_AID_SRC_NAME);
 				_information_events.flags.reset_pos_to_vision = true;
 				resetHorizontalPositionTo(measurement, measurement_var);
@@ -261,14 +254,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 			// Data seems good, attempt a reset
 			ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
 
-			if (_control_status.flags.gps && !pos_xy_fusion_failing) {
+			if ((_control_status.flags.gps || _control_status.flags.mocap) && !pos_xy_fusion_failing) {
 				// reset EV position bias
 				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
 
 			} else {
 				_information_events.flags.reset_pos_to_vision = true;
 
-				if (_control_status.flags.gps) {
+				if (_control_status.flags.gps || _control_status.flags.mocap) {
 					resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
 					_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
 

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -57,6 +57,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
 					     && _control_status.flags.tilt_align
 					     && !_control_status.flags.ev_yaw_fault
+					     && !_control_status.flags.mocap
 					     && PX4_ISFINITE(aid_src.observation)
 					     && PX4_ISFINITE(aid_src.observation_variance);
 

src/modules/ekf2/EKF/mocap.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   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)) {
+
+
+		bool pos_valid = mocap_sample.pos.isAllFinite();
+		//bool q_valid = mocap_sample.q.isAllFinite() && (q.min() > 0.f);
+
+		// yaw_align = false;
+		// _height_sensor_ref = HeightSensor::MOCAP;
+
+		//_control_status.flags.mocap = true;
+
+		// reset position and yaw on init
+
+
+		// position xy
+		if (pos_valid) {
+			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
+
+			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
+		}
+
+		// position z
+		{
+			estimator_aid_source1d_s aid_src{};
+
+			const float pos_obs_var = math::max(sq(mocap_sample.position_var(2)), sq(_param_ekf2_mocap_noise.get()));
+
+			ekf.updateVerticalPositionAidSrcStatus(mocap_sample.time_us,
+							mocap_sample.pos(2),                           // observation
+							pos_obs_var,                                   // observation variance
+							math::max(_param_ekf2_mocap_gate.get(), 1.f),  // innovation gate
+							aid_src);
+
+			if (_param_ekf2_mocap_ctrl.get()) {
+				aid_src.fusion_enabled = true;
+
+				ekf.fuseVerticalPosition(aid_src);
+
+				//const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
+			}
+
+#if defined(MODULE_NAME)
+			aid_src.timestamp = hrt_absolute_time();
+			_estimator_aid_src_mocap_z_pub.publish(aid_src);
+#endif // MODULE_NAME
+		}
+
+	}
+}
+
+#endif // CONFIG_EKF2_MOCAP

src/modules/ekf2/EKF/mocap.hpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_MOCAP_HPP
+#define EKF_MOCAP_HPP
+
+#include "common.h"
+#include "RingBuffer.h"
+
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+
+#if defined(MODULE_NAME)
+# include <px4_platform_common/module_params.h>
+# include <uORB/PublicationMulti.hpp>
+# include <uORB/Subscription.hpp>
+# include <uORB/topics/estimator_aid_source1d.h>
+# include <uORB/topics/estimator_aid_source2d.h>
+# include <uORB/topics/vehicle_odometry.h>
+#endif // MODULE_NAME
+
+class Ekf;
+
+class Mocap : public ModuleParams
+{
+public:
+	Mocap() : ModuleParams(nullptr) {}
+	~Mocap() = default;
+
+	void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
+
+	void updateParameters()
+	{
+		updateParams();
+	}
+
+private:
+
+	struct MocapSample {
+		uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
+		Vector3f    pos{};         ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
+		Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
+		Vector3f    position_var{};    ///< XYZ position variances (m**2)
+		Vector3f    orientation_var{}; ///< orientation variance (rad**2)
+		PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD;
+		uint8_t     reset_counter{};
+		int8_t     quality{};     ///< quality indicator between 0 and 100
+	};
+
+	RingBuffer<MocapSample> _mocap_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
+	uint64_t _time_last_buffer_push{0};
+
+	enum state {
+		stopped,
+		starting,
+		active,
+		stopping,
+		resetting,
+	};
+
+#if defined(MODULE_NAME)
+	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_mocap_xy_pub {ORB_ID(estimator_aid_src_mocap_xy)};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mocap_z_pub{ORB_ID(estimator_aid_src_mocap_z)};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mocap_yaw_pub{ORB_ID(estimator_aid_src_mocap_yaw)};
+
+	uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::EKF2_MOCAP_CTRL>) _param_ekf2_mocap_ctrl,
+		(ParamFloat<px4::params::EKF2_MOCAP_DELAY>) _param_ekf2_mocap_delay,
+		(ParamFloat<px4::params::EKF2_MOCAP_NOISE>) _param_ekf2_mocap_noise,
+		(ParamFloat<px4::params::EKF2_MOCAP_GATE>) _param_ekf2_mocap_gate
+	)
+
+#endif // MODULE_NAME
+};
+
+#endif // CONFIG_EKF2_MOCAP
+
+#endif // !EKF_MOCAP_HPP

src/modules/ekf2/EKF2.cpp

@@ -450,6 +450,8 @@ void EKF2::Run()
 				}
 			}
 		}
+
+		_ekf.updateParameters();
 	}
 
 	if (!_callback_registered) {
@@ -1772,6 +1774,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_mag                     = _ekf.control_status_flags().mag;
 		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
 		status_flags.cs_mag_heading_consistent  = _ekf.control_status_flags().mag_heading_consistent;
+		status_flags.cs_mocap                   = _ekf.control_status_flags().mocap;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/Kconfig

@@ -21,6 +21,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 airspeed fusion support.
 
+menuconfig EKF2_MOCAP
+depends on MODULES_EKF2
+	bool "mocap fusion support"
+	default y
+	---help---
+		EKF2 motion capture (mocap) fusion support.
+
 menuconfig EKF2_AUXVEL
 depends on MODULES_EKF2
         bool "aux velocity fusion support"

src/modules/ekf2/ekf2_params.c

@@ -1568,3 +1568,44 @@ PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0);
  * @decimal 1
  */
 PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
+
+/**
+ * Motion capture (mocap) aiding
+ *
+ * @group EKF2
+ * @boolean
+ */
+PARAM_DEFINE_INT32(EKF2_MOCAP_CTRL, 1);
+
+/**
+ * Motion capture (mocap) delay relative to IMU measurements
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ * @reboot_required true
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_MOCAP_DELAY, 0);
+
+/**
+ * Measurement noise for motion capture (mocap) observations used to lower bound or replace the uncertainty included in the message
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_MOCAP_NOISE, 0.9f);
+
+/**
+ * Gate size for Motion capture (mocap) fusion
+ *
+ * Sets the number of standard deviations used by the innovation consistency test.
+ * @group EKF2
+ * @min 1.0
+ * @unit SD
+ * @decimal 1
+*/
+PARAM_DEFINE_FLOAT(EKF2_MOCAP_GATE, 3.0f);

src/modules/logger/logged_topics.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -198,6 +198,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_mocap", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);