ekf2: select estimator instance manually using RC channel

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -74,6 +74,7 @@ fi
 mc_hover_thrust_estimator start
 flight_mode_manager start
 mc_pos_control start
+fake_ev start
 
 #
 # Start Multicopter Land Detector.

src/modules/ekf2/EKF2.cpp

@@ -46,11 +46,12 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
 #endif // CONFIG_EKF2_MULTI_INSTANCE
 
-EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
+EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, config),
 	_replay_mode(replay_mode && !multi_mode),
 	_multi_mode(multi_mode),
+	_config_param(config_param),
 	_instance(multi_mode ? -1 : 0),
 	_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
 	_local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
@@ -112,7 +113,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_req_vdrift(_params->req_vdrift),
 	_param_ekf2_hgt_ref(_params->height_sensor_ref),
 	_param_ekf2_baro_ctrl(_params->baro_ctrl),
-	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
 	_param_ekf2_noaid_tout(_params->valid_timeout_max),
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_param_ekf2_rng_ctrl(_params->rng_ctrl),
@@ -136,7 +136,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_param_ekf2_ev_delay(_params->ev_delay_ms),
-	_param_ekf2_ev_ctrl(_params->ev_ctrl),
 	_param_ekf2_ev_qmin(_params->ev_quality_minimum),
 	_param_ekf2_evp_noise(_params->ev_pos_noise),
 	_param_ekf2_evv_noise(_params->ev_vel_noise),
@@ -440,6 +439,22 @@ void EKF2::Run()
 				}
 			}
 		}
+
+		if (_config_param == 1) {
+			_params->gnss_ctrl = _param_ekf2_1_gps_ctrl.get();
+
+		} else {
+			_params->gnss_ctrl = _param_ekf2_gps_ctrl.get();
+		}
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_config_param == 1) {
+			_params->ev_ctrl = _param_ekf2_1_ev_ctrl.get();
+
+		} else {
+			_params->ev_ctrl = _param_ekf2_ev_ctrl.get();
+		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	if (!_callback_registered) {
@@ -2560,6 +2575,8 @@ int EKF2::task_spawn(int argc, char *argv[])
 	bool multi_mode = false;
 	int32_t imu_instances = 0;
 	int32_t mag_instances = 0;
+	int32_t conf_instances = 0;
+	param_get(param_find("EKF2_MULTI_CONF"), &conf_instances);
 
 	int32_t sens_imu_mode = 1;
 	param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
@@ -2624,13 +2641,13 @@ int EKF2::task_spawn(int argc, char *argv[])
 		}
 
 		const hrt_abstime time_started = hrt_absolute_time();
-		const int multi_instances = math::min(imu_instances * mag_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
+		const int multi_instances = math::min(imu_instances * mag_instances * conf_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
 		int multi_instances_allocated = 0;
 
 		// allocate EKF2 instances until all found or arming
 		uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
 
-		bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS] {}; // IMUs * mags
+		bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS][MAX_NUM_CONFS] {}; // IMUs * mags * configs
 
 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
@@ -2650,34 +2667,36 @@ int EKF2::task_spawn(int argc, char *argv[])
 					// Mag & IMU data must be valid, first mag can be ignored initially
 					if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
 
-						if (!ekf2_instance_created[imu][mag]) {
-							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
+						for (uint8_t conf = 0; conf < conf_instances; conf++) {
+							if (!ekf2_instance_created[imu][mag][conf]) {
+								EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false, conf);
 
-							if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
-								int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
+								if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
+									int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
 
-								if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
-									_objects[actual_instance].store(ekf2_inst);
-									success = true;
-									multi_instances_allocated++;
-									ekf2_instance_created[imu][mag] = true;
+									if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
+										_objects[actual_instance].store(ekf2_inst);
+										success = true;
+										multi_instances_allocated++;
+										ekf2_instance_created[imu][mag][conf] = true;
 
-									PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
-										  imu, vehicle_imu_sub.get().accel_device_id,
-										  mag, vehicle_mag_sub.get().device_id);
+										PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
+											  imu, vehicle_imu_sub.get().accel_device_id,
+											  mag, vehicle_mag_sub.get().device_id);
 
-									_ekf2_selector.load()->ScheduleNow();
+										_ekf2_selector.load()->ScheduleNow();
+
+									} else {
+										PX4_ERR("instance numbering problem instance: %d", actual_instance);
+										delete ekf2_inst;
+										break;
+									}
 
 								} else {
-									PX4_ERR("instance numbering problem instance: %d", actual_instance);
-									delete ekf2_inst;
+									PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
+									px4_usleep(100000);
 									break;
 								}
-
-							} else {
-								PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
-								px4_usleep(100000);
-								break;
 							}
 						}
 
@@ -2699,7 +2718,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 	{
 		// otherwise launch regular
-		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
+		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode, 0);
 
 		if (ekf2_inst) {
 			_objects[0].store(ekf2_inst);

src/modules/ekf2/EKF2.hpp

@@ -115,7 +115,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	EKF2() = delete;
-	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
+	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param);
 	~EKF2() override;
 
 	/** @see ModuleBase */
@@ -160,6 +160,7 @@ private:
 
 	static constexpr uint8_t MAX_NUM_IMUS = 4;
 	static constexpr uint8_t MAX_NUM_MAGS = 4;
+	static constexpr uint8_t MAX_NUM_CONFS = 2;
 
 	void Run() override;
 
@@ -257,6 +258,7 @@ private:
 
 	const bool _replay_mode{false};			///< true when we use replay data from a log
 	const bool _multi_mode;
+	const int _config_param{0};
 	int _instance{0};
 
 	px4::atomic_bool _task_should_exit{false};
@@ -578,7 +580,8 @@ private:
 		_param_ekf2_aid_mask,		///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
 		(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref,    ///< selects the primary source for height data
 		(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
-		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,  ///< GPS control selection
+		(ParamInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,  ///< GPS control selection
+		(ParamInt<px4::params::EKF2_1_GPS_CTRL>) _param_ekf2_1_gps_ctrl,  ///< Alternate GPS control selection
 
 		(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
 		_param_ekf2_noaid_tout,	///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
@@ -626,7 +629,8 @@ private:
 		(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
 		_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
 
-		(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl,	 ///< external vision (EV) control selection
+		(ParamInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl,	 ///< external vision (EV) control selection
+		(ParamInt<px4::params::EKF2_1_EV_CTRL>) _param_ekf2_1_ev_ctrl,	 ///< Alternate external vision (EV) control selection
 		(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
 		(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
 		(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)

src/modules/ekf2/EKF2Selector.cpp

@@ -754,7 +754,16 @@ void EKF2Selector::Run()
 			}
 		}
 
-		if (!_instance[_selected_instance].healthy.get_state()) {
+		if (_param_ekf2_sel_rc_map.get() > 0) {
+			// Manual instance selection through RC
+			if (isRcOverride()) {
+				SelectInstance(_param_ekf2_sel_rc_inst.get());
+
+			} else {
+				SelectInstance(0);
+			}
+
+		} else if (!_instance[_selected_instance].healthy.get_state()) {
 			// prefer the best healthy instance using a different IMU
 			if (!SelectInstance(best_ekf_different_imu)) {
 				// otherwise switch to the healthy instance with best overall test ratio
@@ -807,6 +816,48 @@ void EKF2Selector::Run()
 	ScheduleDelayed(FILTER_UPDATE_PERIOD);
 }
 
+bool EKF2Selector::isRcOverride()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	float channel = 0;
+
+	switch (_param_ekf2_sel_rc_map.get()) {
+	case 0:
+		return false;
+
+	case 1:
+		channel = manual_control_setpoint.aux1;
+		break;
+
+	case 2:
+		channel = manual_control_setpoint.aux2;
+		break;
+
+	case 3:
+		channel = manual_control_setpoint.aux3;
+		break;
+
+	case 4:
+		channel = manual_control_setpoint.aux4;
+		break;
+
+	case 5:
+		channel = manual_control_setpoint.aux5;
+		break;
+
+	case 6:
+		channel = manual_control_setpoint.aux6;
+		break;
+
+	default:
+		return false;
+	}
+
+	return channel > .5f;
+}
+
 void EKF2Selector::PublishEstimatorSelectorStatus()
 {
 	estimator_selector_status_s selector_status{};

src/modules/ekf2/EKF2Selector.hpp

@@ -47,6 +47,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/sensors_status_imu.h>
@@ -82,6 +83,8 @@ private:
 
 	void Run() override;
 
+	bool isRcOverride();
+
 	void PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance);
 
 	void PublishEstimatorSelectorStatus();
@@ -228,6 +231,7 @@ private:
 	uint8_t _global_position_instance_prev{INVALID_INSTANCE};
 	uint8_t _odometry_instance_prev{INVALID_INSTANCE};
 
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
 
@@ -245,7 +249,9 @@ private:
 		(ParamFloat<px4::params::EKF2_SEL_IMU_RAT>) _param_ekf2_sel_imu_angle_rate,
 		(ParamFloat<px4::params::EKF2_SEL_IMU_ANG>) _param_ekf2_sel_imu_angle,
 		(ParamFloat<px4::params::EKF2_SEL_IMU_ACC>) _param_ekf2_sel_imu_accel,
-		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity
+		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity,
+		(ParamInt<px4::params::EKF2_SEL_RC_MAP>) _param_ekf2_sel_rc_map,
+		(ParamInt<px4::params::EKF2_SEL_RC_INST>) _param_ekf2_sel_rc_inst
 	)
 };
 #endif // !EKF2SELECTOR_HPP

src/modules/ekf2/ekf2_params.c

@@ -686,6 +686,25 @@ PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1);
  */
 PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
 
+/**
+ * (Alt)External vision (EV) sensor aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Horizontal position fusion
+ * 1 : Vertical position fusion
+ * 2 : 3D velocity fusion
+ * 3 : Yaw
+ *
+ * @group EKF2
+ * @min 0
+ * @max 15
+ * @bit 0 Horizontal position
+ * @bit 1 Vertical position
+ * @bit 2 3D velocity
+ * @bit 3 Yaw
+ */
+PARAM_DEFINE_INT32(EKF2_1_EV_CTRL, 15);
+
 /**
  * GNSS sensor aiding
  *
@@ -705,6 +724,25 @@ PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
  */
 PARAM_DEFINE_INT32(EKF2_GPS_CTRL, 7);
 
+/**
+ * (Alt)GNSS sensor aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Longitude and latitude fusion
+ * 1 : Altitude fusion
+ * 2 : 3D velocity fusion
+ * 3 : Dual antenna heading fusion
+ *
+ * @group EKF2
+ * @min 0
+ * @max 15
+ * @bit 0 Lon/lat
+ * @bit 1 Altitude
+ * @bit 2 3D velocity
+ * @bit 3 Dual antenna heading
+ */
+PARAM_DEFINE_INT32(EKF2_1_GPS_CTRL, 7);
+
 /**
  * Range sensor height aiding
  *

src/modules/ekf2/ekf2_params_multi.c

@@ -56,3 +56,17 @@ PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0);
  * @max 4
  */
 PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0);
+
+/**
+ * Multi-EKF Configurations.
+ *
+ * Select the number of configurations to use for Multi-EKF.
+ * Multiple configurations can be used with the same IMU/mag
+ * Requires SENS_IMU_MODE > 0
+ *
+ * @group EKF2
+ * @reboot_required true
+ * @min 1
+ * @max 2
+ */
+PARAM_DEFINE_INT32(EKF2_MULTI_CONF, 1);

src/modules/ekf2/ekf2_params_selector.c

@@ -79,3 +79,39 @@ PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f);
  * @unit m/s
  */
 PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f);
+
+/**
+ * Manual instance selection channel
+ *
+ * Defines which RC_MAP_AUXn parameter maps the RC channel used to select a desired
+ * EKF2 instance.
+ * Use EKF2_SEL_RC_INST to define which instance is selected when this switch is active.
+ * When manual selection is active and the switch is off, instance 0 is selected.
+ * Manual selection overrides automatic instance switch.
+ *
+ * @value 0 Disable
+ * @value 1 Aux1
+ * @value 2 Aux2
+ * @value 3 Aux3
+ * @value 4 Aux4
+ * @value 5 Aux5
+ * @value 6 Aux6
+ * @min 0
+ * @max 6
+ * @group EKF2
+ */
+PARAM_DEFINE_INT32(EKF2_SEL_RC_MAP, 0);
+
+/**
+ * Manual instance selection
+ *
+ * Defines which estimator instance is used when manual switch is active.
+ * Use EKF2_SEL_RC_MAP to define which aux switch is used to trigger the switch.
+ * When manual selection is active and the switch is off, instance 0 is selected.
+ * Manual selection overrides automatic instance switch.
+ *
+ * @min 0
+ * @max 10
+ * @group EKF2
+ */
+PARAM_DEFINE_INT32(EKF2_SEL_RC_INST, 0);

src/modules/fake_ev/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__fake_ev
+	MAIN fake_ev
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		fake_ev.cpp
+		fake_ev.hpp
+	DEPENDS
+		px4_work_queue
+		mathlib
+)

src/modules/fake_ev/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_FAKE_EV
+	bool "fake_ev"
+	default y
+	---help---
+		Enable fake external vision publisher
+
+menuconfig USER_FAKE_EV
+	bool "fake_ev running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_FAKE_EV
+	---help---
+		Put fake_ev in userspace memory

src/modules/fake_ev/fake_ev.cpp

@@ -0,0 +1,211 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fake_ev.cpp
+ */
+
+#include "fake_ev.hpp"
+
+#include <mathlib/mathlib.h>
+
+FakeEV::FakeEV() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	updateParams();
+}
+
+FakeEV::~FakeEV()
+{
+	perf_free(_cycle_perf);
+}
+
+bool FakeEV::init()
+{
+	if (!_vehicle_gps_position_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+void FakeEV::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void FakeEV::Run()
+{
+	if (should_exit()) {
+		_vehicle_gps_position_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_vehicle_gps_position_sub.updated()) {
+		return;
+	}
+
+	perf_begin(_cycle_perf);
+
+	sensor_gps_s gps{};
+
+	if (_vehicle_gps_position_sub.copy(&gps)) {
+		vehicle_odometry_s odom = gpsToOdom(gps);
+		_vehicle_visual_odometry_pub.publish(odom);
+	}
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+
+	perf_end(_cycle_perf);
+}
+
+vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps)
+{
+	vehicle_odometry_s odom{vehicle_odometry_empty};
+
+	/* odom.timestamp_sample = gps.timestamp_sample; */ // gps timestamp_sample isn't set in SITL
+	odom.timestamp_sample = gps.timestamp;
+
+	const hrt_abstime now = odom.timestamp_sample;
+	const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.5f);
+	_last_run = now;
+
+	if (gps.fix_type >= 3) {
+		if (PX4_ISFINITE(_alt_ref)) {
+			matrix::Vector2f hpos = _hpos_prev;
+
+			if (!_param_fev_stale.get()) {
+				hpos = _pos_ref.project(gps.lat / 1.0e7, gps.lon / 1.0e7);
+				_hpos_prev = hpos;
+			}
+
+			_h_drift += _param_fev_h_drift_rate.get() * dt;
+			hpos += matrix::Vector2f(_h_drift, _h_drift);
+			const float vpos = -((float)gps.alt * 1e-3f - _alt_ref);
+			matrix::Vector3f(hpos(0), hpos(1), vpos).copyTo(odom.position);
+
+			const float hvar = std::pow(gps.eph, 2.f);
+			const float vvar = std::pow(gps.epv, 2.f);
+			matrix::Vector3f(hvar, hvar, vvar).copyTo(odom.position_variance);
+
+			odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
+
+		} else {
+			_pos_ref.initReference(gps.lat / 1.0e7, gps.lon / 1.0e7, gps.timestamp);
+			_alt_ref = (float)gps.alt * 1e-3f;
+		}
+	}
+
+	if (gps.vel_ned_valid) {
+		matrix::Vector3f(gps.vel_n_m_s, gps.vel_e_m_s, gps.vel_d_m_s).copyTo(odom.velocity);
+
+		matrix::Vector3f vel_var;
+		vel_var.setAll(std::pow(gps.s_variance_m_s, 2.f));
+		vel_var.copyTo(odom.velocity_variance);
+		odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
+	}
+
+	odom.timestamp = hrt_absolute_time();
+
+	return odom;
+}
+
+int FakeEV::task_spawn(int argc, char *argv[])
+{
+	FakeEV *instance = new FakeEV();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int FakeEV::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FakeEV::print_status()
+{
+	perf_print_counter(_cycle_perf);
+
+	return 0;
+}
+
+int FakeEV::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fake_ev", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fake_ev_main(int argc, char *argv[])
+{
+	return FakeEV::main(argc, argv);
+}

src/modules/fake_ev/fake_ev.hpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fake_ev.hpp
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_odometry.h>
+
+using namespace time_literals;
+
+class FakeEV : public ModuleBase<FakeEV>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FakeEV();
+	~FakeEV() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	static constexpr vehicle_odometry_s vehicle_odometry_empty {
+		.timestamp = 0,
+		.timestamp_sample = 0,
+		.position = {NAN, NAN, NAN},
+		.q = {NAN, NAN, NAN, NAN},
+		.velocity = {NAN, NAN, NAN},
+		.angular_velocity = {NAN, NAN, NAN},
+		.position_variance = {NAN, NAN, NAN},
+		.orientation_variance = {NAN, NAN, NAN},
+		.velocity_variance = {NAN, NAN, NAN},
+		.pose_frame = vehicle_odometry_s::POSE_FRAME_UNKNOWN,
+		.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_UNKNOWN,
+		.reset_counter = 0,
+		.quality = 0
+	};
+
+	void Run() override;
+	void updateParams() override;
+
+	vehicle_odometry_s gpsToOdom(const sensor_gps_s &gps);
+
+	uORB::Publication<vehicle_odometry_s> _vehicle_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	hrt_abstime _timestamp_last{0};
+
+	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude
+	float _alt_ref{NAN};
+
+	hrt_abstime _last_run{0};
+	float _h_drift{0.f};
+	float _h_drift_rate{0.05f};
+
+	bool _is_stale{false};
+	matrix::Vector2f _hpos_prev;
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::FEV_EN>) _param_fev_en,
+		(ParamBool<px4::params::FEV_STALE>) _param_fev_stale,
+		(ParamFloat<px4::params::FEV_H_DRIFT_RATE>) _param_fev_h_drift_rate
+	)
+};

src/modules/fake_ev/fake_ev_params.c

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fake_ev_params.c
+ */
+
+
+/**
+ * Enable fake EV
+ *
+ * Generates fake vision output based on GNSS data
+ *
+ * @boolean
+ * @group Fake EV
+ */
+PARAM_DEFINE_INT32(FEV_EN, 1);
+
+/**
+ * Stale sensor
+ *
+ * Enable to freeze the current output
+ *
+ * @boolean
+ * @group Fake EV
+ */
+PARAM_DEFINE_INT32(FEV_STALE, 0);
+
+/**
+ * Drift rate
+ *
+ * Set the horizontal drift rate
+ *
+ * @min 0
+ * @max 5
+ * @decimal 2
+ * @unit m/s
+ * @group Fake EV
+ */
+PARAM_DEFINE_FLOAT(FEV_H_DRIFT_RATE, 0);