TEST: upgrade cache GitHub action to v4

* reset position by fusion

* handle local_pos_valid for fixed wing in gnss denied

* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

* global origin, also reset vertical pos without gps as ref

* fix wo gnss, that bitcraze ci passes

* revert some changes as requested

* remove duplicate reset messages

* undo unrelated whitespace changes, I'll fix it everywhere in a followup

* [SQUASH] ekf2: add vehicle_command_ack

* resetGlobalPosToExternalObservation consolidate logic

* remove gnss check from local_pos validation check

* reset when  0<accuracy<1, otherwise fuse

* replace gps param with flag

* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected

* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>

.github/workflows/mavros_mission_tests.yml

@@ -4,6 +4,7 @@ on:
   push:
     branches:
     - 'main'
+    - '*'
   pull_request:
     branches:
     - '*'
@@ -25,7 +26,7 @@ jobs:
           #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
 
     container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
+      image: px4io/px4-dev-ros-melodic:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
@@ -39,7 +40,7 @@ jobs:
         string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
         message("::set-output name=timestamp::${current_date}")
     - name: ccache cache files
-      uses: actions/cache@v2
+      uses: actions/cache@v4
       with:
         path: ~/.ccache
         key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -7,6 +7,8 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 #
 
 . ${R}etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21

@@ -13,6 +13,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board diatone_mamba-f405-mk2 exclude
 #
 . ${R}etc/init.d/rc.mc_defaults

boards/px4/fmu-v6xrt/src/mtd.cpp

@@ -53,18 +53,12 @@ static const px4_mft_device_t i2c6 = {             // 24LC64T on BASE  8K 32 X 2
 
 static const px4_mtd_entry_t fmum_fram = {
 	.device = &qspi_flash,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = 256
 		}
 	},
 };

msg/BatteryStatus.msg

@@ -66,12 +66,8 @@ uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're ful
 
 uint8 MAX_INSTANCES = 4
 
-float32 average_power               # The average power of the current discharge
-float32 available_energy            # The predicted charge or energy remaining in the battery
 float32 full_charge_capacity_wh     # The compensated battery capacity
 float32 remaining_capacity_wh       # The compensated battery capacity remaining
-float32 design_capacity             # The design capacity of the battery
-uint16 average_time_to_full         # The predicted remaining time until the battery reaches full charge, in minutes
 uint16 over_discharge_count         # Number of battery overdischarge
 float32 nominal_voltage             # Nominal voltage of the battery pack
 

msg/VehicleCommand.msg

@@ -102,7 +102,6 @@ uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibrati
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 
 uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning
-uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004
 
 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -37,12 +37,11 @@
  * Client-side implementation of UDRAL specification ESC service
  *
  * Publishes the following Cyphal messages:
- *   reg.drone.service.actuator.common.sp.Value8.0.1
- *   reg.drone.service.common.Readiness.0.1
+ *   reg.udral.service.actuator.common.sp.Value31.0.1
+ *   reg.udral.service.common.Readiness.0.1
  *
  * Subscribes to the following Cyphal messages:
- *   reg.drone.service.actuator.common.Feedback.0.1
- *   reg.drone.service.actuator.common.Status.0.1
+ *   zubax.telega.CompactFeedback.0.1
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  * @author Jacob Crabill <jacob@flyvoly.com>
@@ -51,11 +50,13 @@
 #pragma once
 
 #include <lib/perf/perf_counter.h>
-#include <uORB/PublicationMulti.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
+#include "../Subscribers/DynamicPortSubscriber.hpp"
+#include "../Publishers/Publisher.hpp"
 #include <lib/mixer_module/mixer_module.hpp>
 
 // UDRAL Specification Messages
@@ -63,16 +64,15 @@ using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>
 
-/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
-class UavcanEscController : public UavcanPublisher
+
+class ReadinessPublisher : public UavcanPublisher
 {
 public:
-	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
-	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral.", "esc") { };
+	ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "readiness") { };
 
-	~UavcanEscController() {};
+	~ReadinessPublisher() {};
 
 	void update() override
 	{
@@ -95,58 +95,18 @@ public:
 		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
 			publish_readiness();
 		}
-	};
+	}
 
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
-	{
-		if (_port_id > 0) {
-			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
-			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;
 
-			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
-				if (i < num_outputs) {
-					msg_sp.value[i] = static_cast<float>(outputs[i]);
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	actuator_armed_s _armed {};
 
-				} else {
-					// "unset" values published as NaN
-					msg_sp.value[i] = NAN;
-				}
-			}
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};
 
-			uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
-
-			const CanardTransferMetadata transfer_metadata = {
-				.priority       = CanardPriorityNominal,
-				.transfer_kind  = CanardTransferKindMessage,
-				.port_id        = _port_id,                // This is the subject-ID.
-				.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
-				.transfer_id    = _transfer_id,
-			};
-
-			int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
-					&payload_size);
-
-			if (result == 0) {
-				// set the data ready in the buffer and chop if needed
-				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-				result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-							       &transfer_metadata,
-							       payload_size,
-							       &esc_sp_payload_buffer);
-			}
-		}
-	};
-
-	/**
-	 * Sets the number of rotors
-	 */
-	void set_rotor_count(uint8_t count) { _rotor_count = count; }
-
-private:
-	/**
-	 * ESC status message reception will be reported via this callback.
-	 */
-	void esc_status_sub_cb(const CanardRxTransfer &msg);
+	CanardTransferID _arming_transfer_id;
 
 	void publish_readiness()
 	{
@@ -155,8 +115,7 @@ private:
 
 		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
-		// Only publish if we have a valid publication ID set
-		if (_port_id == 0) {
+		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
 			return;
 		}
 
@@ -174,12 +133,12 @@ private:
 
 		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
 
-		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id));
 		const CanardTransferMetadata transfer_metadata = {
 			.priority       = CanardPriorityNominal,
 			.transfer_kind  = CanardTransferKindMessage,
-			.port_id        = arming_pid,                // This is the subject-ID.
-			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+			.port_id        = arming_pid,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
 			.transfer_id    = _arming_transfer_id,
 		};
 
@@ -187,25 +146,143 @@ private:
 				&payload_size);
 
 		if (result == 0) {
-			// set the data ready in the buffer and chop if needed
-			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			++_arming_transfer_id;
 			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 						       &transfer_metadata,
 						       payload_size,
 						       &arming_payload_buffer);
 		}
 	};
-
-	uint8_t _rotor_count {0};
-
-	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
-	hrt_abstime _previous_pub_time = 0;
-
-	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
-	actuator_armed_s _armed {};
-
-	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
-	uint64_t _actuator_test_timestamp{0};
-
-	CanardTransferID _arming_transfer_id;
+};
+
+class UavcanEscController : public UavcanPublisher
+{
+public:
+	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+
+	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "esc") { }
+
+	~UavcanEscController() {}
+
+	void update() override
+	{
+	}
+
+	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+	{
+		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
+			return;
+		}
+
+		uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS;
+
+		for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) {
+			if (outputs[i] != 0) {
+				_max_number_of_nonzero_outputs = i + 1;
+				break;
+			}
+		}
+
+		uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_];
+
+		for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) {
+			payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0);
+		}
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = _port_id,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
+			.transfer_id    = _transfer_id,
+		};
+
+		++_transfer_id;
+		_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+				      &transfer_metadata,
+				      _max_number_of_nonzero_outputs * 2,
+				      &payload_buffer);
+	}
+
+private:
+	uint8_t _max_number_of_nonzero_outputs{1};
+};
+
+class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanEscFeedbackSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "zubax.", "feedback", instance) {}
+
+	void subscribe() override
+	{
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+		_esc_status.esc_armed_flags |= 1 << _instance;
+		_esc_status.esc_count++;
+	};
+
+	void unsubscribe() override
+	{
+		_canard_handle.RxUnsubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id);
+		_esc_status.esc_armed_flags &= ~(1 << _instance);
+		_esc_status.esc_count--;
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		if (_instance >= esc_status_s::CONNECTED_ESC_MAX) {
+			return;
+		}
+
+		auto &ref = _esc_status.esc[_instance];
+		const ZubaxCompactFeedback *feedback = ((const ZubaxCompactFeedback *)(receive.payload));
+
+		ref.timestamp       = hrt_absolute_time();
+		ref.esc_address     = receive.metadata.remote_node_id;
+		ref.esc_voltage     = 0.2 * feedback->dc_voltage;
+		ref.esc_current     = 0.2 * feedback->dc_current;
+		ref.esc_temperature = NAN;
+		ref.esc_rpm         = feedback->velocity * RAD_PER_SEC_TO_RPM;
+		ref.esc_errorcount  = 0;
+
+		_esc_status.counter++;
+		_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+		_esc_status.esc_armed_flags = (1 << _esc_status.esc_count) - 1;
+		_esc_status.timestamp = hrt_absolute_time();
+		_esc_status_pub.publish(_esc_status);
+
+		_esc_status.esc_online_flags = 0;
+		const hrt_abstime now = hrt_absolute_time();
+
+		for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+			if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) {
+				_esc_status.esc_online_flags |= (1 << index);
+			}
+		}
+	};
+
+private:
+	static constexpr float RAD_PER_SEC_TO_RPM = 9.5492968;
+	static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7;
+
+	// https://telega.zubax.com/interfaces/cyphal.html#compact
+#pragma pack(push, 1)
+	struct ZubaxCompactFeedback {
+		uint32_t dc_voltage 		: 11;
+		int32_t dc_current 		: 12;
+		int32_t phase_current_amplitude	: 12;
+		int32_t velocity 		: 13;
+		int8_t demand_factor_pct 	: 8;
+	};
+#pragma pack(pop)
+	static_assert(sizeof(ZubaxCompactFeedback) == zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES);
+
+	static esc_status_s _esc_status;
+
+	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
 };

src/drivers/cyphal/Cyphal.cpp

@@ -62,6 +62,8 @@ using namespace time_literals;
 
 CyphalNode *CyphalNode::_instance;
 
+esc_status_s UavcanEscFeedbackSubscriber::_esc_status;
+
 CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
@@ -125,7 +127,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);
 
 	} else {
-		_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
+		_instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC);
 	}
 
 	if (_instance == nullptr) {
@@ -188,6 +190,8 @@ void CyphalNode::Run()
 		// send uavcan::node::Heartbeat_1_0 @ 1 Hz
 		sendHeartbeat();
 
+		sendPortList();
+
 		// Check all publishers
 		_pub_manager.update();
 
@@ -359,10 +363,10 @@ void CyphalNode::sendHeartbeat()
 	if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {
 
 		uavcan_node_Heartbeat_1_0 heartbeat{};
-		heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
+		const hrt_abstime now = hrt_absolute_time();
+		heartbeat.uptime = now / 1000000;
 		heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
 		heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
-		const hrt_abstime now = hrt_absolute_time();
 		size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
 		const CanardTransferMetadata transfer_metadata = {
@@ -392,6 +396,45 @@ void CyphalNode::sendHeartbeat()
 	}
 }
 
+void CyphalNode::sendPortList()
+{
+	static hrt_abstime _uavcan_node_port_List_last{0};
+
+	if (hrt_elapsed_time(&_uavcan_node_port_List_last) < 3_s) {
+		return;
+	}
+
+	static uavcan_node_port_List_0_1 msg{};
+	static uint8_t uavcan_node_port_List_0_1_buffer[uavcan_node_port_List_0_1_EXTENT_BYTES_];
+	static CanardTransferID _uavcan_node_port_List_transfer_id{0};
+	size_t payload_size = uavcan_node_port_List_0_1_EXTENT_BYTES_;
+	const hrt_abstime now = hrt_absolute_time();
+
+	const CanardTransferMetadata transfer_metadata = {
+		.priority       = CanardPriorityNominal,
+		.transfer_kind  = CanardTransferKindMessage,
+		.port_id        = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
+		.remote_node_id = CANARD_NODE_ID_UNSET,
+		.transfer_id    = _uavcan_node_port_List_transfer_id++
+	};
+
+	// memset(uavcan_node_port_List_0_1_buffer, 0x00, uavcan_node_port_List_0_1_EXTENT_BYTES_);
+	uavcan_node_port_List_0_1_initialize_(&msg);
+
+	_pub_manager.fillSubjectIdList(msg.publishers);
+	_sub_manager.fillSubjectIdList(msg.subscribers);
+
+	uavcan_node_port_List_0_1_serialize_(&msg, uavcan_node_port_List_0_1_buffer, &payload_size);
+
+	_canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+			      &transfer_metadata,
+			      payload_size,
+			      &uavcan_node_port_List_0_1_buffer
+			     );
+
+	_uavcan_node_port_List_last = now;
+}
+
 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {

src/drivers/cyphal/Cyphal.hpp

@@ -137,6 +137,9 @@ private:
 	// Sends a heartbeat at 1s intervals
 	void sendHeartbeat();
 
+	// Sends a port.List at 3s intervals
+	void sendPortList();
+
 	px4::atomic_bool _task_should_exit{false};	///< flag to indicate to tear down the CAN driver
 
 	bool _initialized{false};		///< number of actuators currently available

src/drivers/cyphal/ParamManager.cpp

@@ -56,6 +56,15 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
 		}
 	}
 
+	for (auto &param : _type_registers) {
+		if (strcmp(param_name, param.name) == 0) {
+			uavcan_register_Value_1_0_select_string_(&value);
+			value._string.value.count = strlen(param.value);
+			memcpy(&value._string, param.value, value._string.value.count);
+			return true;
+		}
+	}
+
 	return false;
 }
 
@@ -73,19 +82,36 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 		}
 	}
 
+	for (auto &param : _type_registers) {
+		if (strncmp((char *)name.name.elements, param.name, name.name.count) == 0) {
+			uavcan_register_Value_1_0_select_string_(&value);
+			value._string.value.count = strlen(param.value);
+			memcpy(&value._string, param.value, value._string.value.count);
+			return true;
+		}
+	}
+
 	return false;
 }
 
 bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name)
 {
-	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+	size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder);
+	size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister);
+
+	if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+		strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
+		name.name.count = strlen(_uavcan_params[id].uavcan_name);
+
+	} else if (id < number_of_integer_registers + number_of_type_registers) {
+		id -= number_of_integer_registers;
+		strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name));
+		name.name.count = strlen(_type_registers[id].name);
+
+	} else {
 		return false;
 	}
 
-	strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
-
-	name.name.count = strlen(_uavcan_params[id].uavcan_name);
-
 	return true;
 }
 

src/drivers/cyphal/ParamManager.hpp

@@ -103,6 +103,10 @@ typedef struct {
 	bool is_persistent {true};
 } UavcanParamBinder;
 
+typedef struct {
+	const char *name;
+	const char *value;
+} CyphalTypeRegister;
 
 class UavcanParamManager
 {
@@ -116,8 +120,9 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[13] {
+	const UavcanParamBinder _uavcan_params[22] {
 		{"uavcan.pub.udral.esc.0.id",                	"UCAN1_ESC_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.readiness.0.id",            	"UCAN1_READ_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.servo.0.id",              	"UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.gps.0.id",                	"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.actuator_outputs.0.id",   	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -130,7 +135,28 @@ private:
 		{"uavcan.sub.udral.legacy_bms.0.id",         	"UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.0.id",            	"UCAN1_FB0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.1.id",            	"UCAN1_FB1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.2.id",            	"UCAN1_FB2_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.3.id",            	"UCAN1_FB3_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.4.id",            	"UCAN1_FB4_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.5.id",            	"UCAN1_FB5_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.6.id",            	"UCAN1_FB6_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.7.id",            	"UCAN1_FB7_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};
+
+	CyphalTypeRegister _type_registers[10] {
+		{"uavcan.pub.udral.esc.0.type", 		"reg.udral.service.actuator.common.sp.Vector31"},
+		{"uavcan.pub.udral.readiness.0.type", 		"reg.udral.service.common.Readiness.0.1"},
+		{"uavcan.sub.zubax.feedback.0.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.1.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.2.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.3.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.4.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.5.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.6.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.7.type", 		"zubax.telega.CompactFeedback.0.1"},
+	};
 };

src/drivers/cyphal/PublicationManager.cpp

@@ -125,6 +125,15 @@ UavcanPublisher *PublicationManager::getPublisher(const char *subject_name)
 	return NULL;
 }
 
+void PublicationManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list)
+{
+	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&publishers_list);
+
+	for (auto &dynpub : _dynpublishers) {
+		publishers_list.sparse_list.elements[publishers_list.sparse_list.count].value = dynpub->id();
+		publishers_list.sparse_list.count++;
+	}
+}
 
 void PublicationManager::update()
 {

src/drivers/cyphal/PublicationManager.hpp

@@ -67,7 +67,7 @@
 /* Preprocessor calculation of publisher count */
 
 #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
-	CONFIG_CYPHAL_ESC_CONTROLLER + \
+	2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@@ -79,6 +79,7 @@
 
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uavcan/node/port/List_0_1.h>
 
 #include "Actuators/EscClient.hpp"
 #include "Publishers/udral/Readiness.hpp"
@@ -103,6 +104,7 @@ public:
 
 	UavcanPublisher *getPublisher(const char *subject_name);
 
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list);
 private:
 	void updateDynamicPublications();
 
@@ -131,6 +133,14 @@ private:
 			"udral.esc",
 			0
 		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new ReadinessPublisher(handle, pmgr);
+			},
+			"udral.readiness",
+			0
+		},
 #endif
 #if CONFIG_CYPHAL_READINESS_PUBLISHER
 		{

src/drivers/cyphal/SubscriptionManager.cpp

@@ -158,3 +158,24 @@ void SubscriptionManager::updateParams()
 	// Check for any newly-enabled subscriptions
 	updateDynamicSubscriptions();
 }
+
+void SubscriptionManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list)
+{
+	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&subscribers_list);
+
+	UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+	auto &sparse_list = subscribers_list.sparse_list;
+
+	while (dynsub != nullptr) {
+		int32_t instance_idx = 0;
+
+		while (dynsub->isValidPortId(dynsub->id(instance_idx))) {
+			sparse_list.elements[sparse_list.count].value = dynsub->id(instance_idx);
+			sparse_list.count++;
+			instance_idx++;
+		}
+
+		dynsub = dynsub->next();
+	}
+}

src/drivers/cyphal/SubscriptionManager.hpp

@@ -45,6 +45,10 @@
 #define CONFIG_CYPHAL_ESC_SUBSCRIBER 0
 #endif
 
+#ifndef CONFIG_CYPHAL_ESC_CONTROLLER
+#define CONFIG_CYPHAL_ESC_CONTROLLER 0
+#endif
+
 #ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 #define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0
 #endif
@@ -65,12 +69,15 @@
 
 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
+	8 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
 
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
+#include <uavcan/node/port/List_0_1.h>
+#include "Actuators/EscClient.hpp"
 #include "Subscribers/DynamicPortSubscriber.hpp"
 #include "CanardInterface.hpp"
 
@@ -100,6 +107,7 @@ public:
 	void subscribe();
 	void printInfo();
 	void updateParams();
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list);
 
 private:
 	void updateDynamicSubscriptions();
@@ -130,6 +138,72 @@ private:
 			0
 		},
 #endif
+#if CONFIG_CYPHAL_ESC_CONTROLLER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 0);
+			},
+			"zubax.feedback",
+			0
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 1);
+			},
+			"zubax.feedback",
+			1
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 2);
+			},
+			"zubax.feedback",
+			2
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 3);
+			},
+			"zubax.feedback",
+			3
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 4);
+			},
+			"zubax.feedback",
+			4
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 5);
+			},
+			"zubax.feedback",
+			5
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 6);
+			},
+			"zubax.feedback",
+			6
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 7);
+			},
+			"zubax.feedback",
+			7
+		},
+#endif
 #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 		{
 			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *

src/drivers/cyphal/parameters.c

@@ -162,6 +162,87 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
  */
 PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
 
+/**
+ * Cyphal ESC readiness port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1);
+
+/**
+ * Cyphal ESC 0 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB0_SUB, -1);
+
+/**
+ * Cyphal ESC 1 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB1_SUB, -1);
+
+/**
+ * Cyphal ESC 2 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB2_SUB, -1);
+
+/**
+ * Cyphal ESC 3 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB3_SUB, -1);
+
+/**
+ * Cyphal ESC 4 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB4_SUB, -1);
+
+/**
+ * Cyphal ESC 5 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB5_SUB, -1);
+
+/**
+ * Cyphal ESC 6 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB6_SUB, -1);
+
+/**
+ * Cyphal ESC 7 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB7_SUB, -1);
+
 /**
  * Cyphal GPS publication port ID.
  *

src/lib/battery/int_res_est_replay.py

@@ -19,6 +19,13 @@ def getData(log, topic_name, variable_name, instance=0):
 def us2s(time_us):
     return time_us * 1e-6
 
+def getParam(log, param_name):
+    if param_name in log.initial_parameters:
+        return log.initial_parameters[param_name]
+    else:
+        print(f"Parameter {param_name} not found in log.")
+    return None
+
 def rls_update(theta, P, x, V, I, lam):
     gamma = P @ x / (lam + x.T @ P @ x)
     error = V - x.T @ theta
@@ -33,6 +40,31 @@ def rls_update(theta, P, x, V, I, lam):
 
 def main(log_name, n_cells, full_cell, empty_cell, lam):
     log = ULog(log_name)
+
+    log_n_cells = getParam(log, 'BAT1_N_CELLS')
+    log_full_cell = getParam(log, 'BAT1_V_CHARGED')
+    log_empty_cell = getParam(log, 'BAT1_V_EMPTY')
+
+    # Debug information
+    print(f"Extracted from log - BAT1_N_CELLS: {log_n_cells}, BAT1_V_CHARGED: {log_full_cell}, BAT1_V_EMPTY: {log_empty_cell}")
+
+    # Use log parameters unless overridden
+    if n_cells is None:
+        n_cells = log_n_cells
+    else:
+        print(f"Using override for n_cells: {n_cells}")
+    if full_cell is None:
+        full_cell = log_full_cell
+    else:
+        print(f"Using override for full_cell: {full_cell}")
+    if empty_cell is None:
+        empty_cell = log_empty_cell
+    else:
+        print(f"Using override for empty_cell: {empty_cell}")
+
+    # Debug information for final parameter values
+    print(f"Using parameters - n_cells: {n_cells}, full_cell: {full_cell}, empty_cell: {empty_cell}")
+
     timestamps = us2s(getData(log, 'battery_status', 'timestamp'))
     I = getData(log, 'battery_status', 'current_a')
     V = getData(log, 'battery_status', 'voltage_v')
@@ -79,7 +111,7 @@ def main(log_name, n_cells, full_cell, empty_cell, lam):
         data_cov_hist[index] = data_cov
         internal_resistance_stable[index] = max(R_est[index]/n_cells, 0.001)
 
-    ## Plot data
+    # Plot data
     print("Internal Resistance mean (per cell): ", np.mean(R_est) / n_cells)
 
     # Summary plot
@@ -168,9 +200,9 @@ def main(log_name, n_cells, full_cell, empty_cell, lam):
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(description='Estimate battery parameters from ulog file.')
     parser.add_argument('-f', type = str,   required = True,  help = 'Full path to ulog file')
-    parser.add_argument('-c', type = float, required = True,  help = 'Number of cells in battery')
-    parser.add_argument('-u', type = float, required = False, default = 4.05, help = 'Full cell voltage')
-    parser.add_argument('-e', type = float, required = False, default = 3.6,  help = 'Empty cell voltage')
+    parser.add_argument('-c', type = float, required = False, help = 'Number of cells in battery')
+    parser.add_argument('-u', type = float, required = False, default = None, help = 'Full cell voltage')
+    parser.add_argument('-e', type = float, required = False, default = None,  help = 'Empty cell voltage')
     parser.add_argument('-l', type = float, required = False, default = 0.99, help = 'Forgetting factor')
     args = parser.parse_args()
     main(args.f, args.c, args.u, args.e, args.l)

src/modules/ekf2/CMakeLists.txt

@@ -216,10 +216,6 @@ if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS EKF/terrain_control.cpp)
 endif()
 
-if(CONFIG_EKF2_WIND)
-	list(APPEND EKF_SRCS EKF/wind.cpp)
-endif ()
-
 add_subdirectory(EKF)
 
 px4_add_module(

src/modules/ekf2/EKF/CMakeLists.txt

@@ -138,10 +138,6 @@ if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS terrain_control.cpp)
 endif()
 
-if(CONFIG_EKF2_WIND)
-	list(APPEND EKF_SRCS wind.cpp)
-endif ()
-
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_library(ecl_EKF
 	${EKF_SRCS}

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp

@@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate()
 
 void ZeroVelocityUpdate::reset()
 {
-	_time_last_zero_velocity_fuse = 0;
+	_time_last_fuse = 0;
 }
 
 bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
 	// Fuse zero velocity at a limited rate (every 200 milliseconds)
-	const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us);
+	const bool zero_velocity_update_data_ready = (_time_last_fuse + 200'000 < imu_delayed.time_us);
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest
@@ -69,7 +69,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}
 
-			_time_last_zero_velocity_fuse = imu_delayed.time_us;
+			_time_last_fuse = imu_delayed.time_us;
 
 			return true;
 		}

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp

@@ -45,9 +45,11 @@ public:
 	void reset() override;
 	bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;
 
+	const auto &time_last_fuse() const { return _time_last_fuse; }
+
 private:
 
-	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
+	uint64_t _time_last_fuse{0}; ///< last time of zero velocity update (uSec)
 
 };
 

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -127,7 +127,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
 				resetVelUsingAirspeed(airspeed_sample);
 
-			} else if (!_control_status.flags.wind) {
+			} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
 				// If starting wind state estimation, reset the wind states and covariances before fusing any data
 				// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
 				resetWindUsingAirspeed(airspeed_sample);

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -57,10 +57,6 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
 
 		if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) {
 			fuseDrag(drag_sample);
-
-			if (!_aid_src_drag.fused && !_control_status.flags.fuse_aspd && !_control_status.flags.fuse_beta) {
-				resetWind();
-			}
 		}
 	}
 }

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -37,6 +37,9 @@
  */
 
 #include "ekf.h"
+#include <ekf_derivation/generated/compute_ev_body_vel_hx.h>
+#include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
+#include <ekf_derivation/generated/compute_ev_body_vel_hz.h>
 
 void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
 			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
@@ -57,14 +60,16 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
 	// rotate measurement into correct earth frame if required
-	Vector3f vel{NAN, NAN, NAN};
-	Matrix3f vel_cov{};
+	Vector3f measurement{};
+	Vector3f measurement_var{};
+
+	float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise));
 
 	switch (ev_sample.vel_frame) {
 	case VelocityFrame::LOCAL_FRAME_NED:
 		if (_control_status.flags.yaw_align) {
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
 
 		} else {
 			continuing_conditions_passing = false;
@@ -75,31 +80,28 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	case VelocityFrame::LOCAL_FRAME_FRD:
 		if (_control_status.flags.ev_yaw) {
 			// using EV frame
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
 
 		} else {
 			// rotate EV to the EKF reference frame
 			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
 
-			vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
-			vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();
-
-			// increase minimum variance to include EV orientation variance
-			// TODO: do this properly
-			const float orientation_var_max = ev_sample.orientation_var.max();
-
-			for (int i = 0; i < 2; i++) {
-				vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
-			}
+			measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
+			measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) *
+					  R_ev_to_ekf.transpose()).diag();
+			minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max());
 		}
 
 		break;
 
-	case VelocityFrame::BODY_FRAME_FRD:
-		vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
-		vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
-		break;
+	case VelocityFrame::BODY_FRAME_FRD: {
+
+			// currently it is assumed that the orientation of the EV frame and the body frame are the same
+			measurement = ev_sample.vel - vel_offset_body;
+			measurement_var = ev_sample.velocity_var;
+			break;
+		}
 
 	default:
 		continuing_conditions_passing = false;
@@ -111,48 +113,56 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
-			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
+			measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise));
 		}
 	}
 
 #endif // CONFIG_EKF2_GNSS
 
-	const Vector3f measurement{vel};
-
-	const Vector3f measurement_var{
-		math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
-		math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
-		math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
+	measurement_var = Vector3f{
+		math::max(measurement_var(0), minimum_variance),
+		math::max(measurement_var(1), minimum_variance),
+		math::max(measurement_var(2), minimum_variance)
 	};
+	continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite();
 
-	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
+	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+		const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
+		const uint64_t t = aid_src.timestamp_sample;
+		updateAidSourceStatus(aid_src,
+				      ev_sample.time_us,					// sample timestamp
+				      measurement_ekf_frame,					// observation
+				      measurement_var_ekf_frame,				// observation variance
+				      _state.vel - measurement_ekf_frame,			// innovation
+				      getVelocityVariance() + measurement_var_ekf_frame,	// innovation variance
+				      math::max(_params.ev_vel_innov_gate, 1.f));		// innovation gate
+		aid_src.timestamp_sample = t;
+		measurement.copyTo(aid_src.observation);
+		measurement_var.copyTo(aid_src.observation_variance);
 
-	updateAidSourceStatus(aid_src,
-				 ev_sample.time_us,                          // sample timestamp
-				 measurement,                                // observation
-				 measurement_var,                            // observation variance
-				 _state.vel - measurement,                   // innovation
-				 getVelocityVariance() + measurement_var,    // innovation variance
-				 math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
-
-	if (!measurement_valid) {
-		continuing_conditions_passing = false;
+	} else {
+		updateAidSourceStatus(aid_src,
+				      ev_sample.time_us,				// sample timestamp
+				      measurement,					// observation
+				      measurement_var,					// observation variance
+				      _state.vel - measurement,				// innovation
+				      getVelocityVariance() + measurement_var,		// innovation variance
+				      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
 	}
 
+
 	const bool starting_conditions_passing = common_starting_conditions_passing
 			&& continuing_conditions_passing
 			&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
 
 	if (_control_status.flags.ev_vel) {
-
 		if (continuing_conditions_passing) {
-
 			if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
-
 				if (quality_sufficient) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					_information_events.flags.reset_vel_to_vision = true;
-					resetVelocityTo(measurement, measurement_var);
+					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 					resetAidSourceStatusZeroInnovation(aid_src);
 
 				} else {
@@ -163,7 +173,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				}
 
 			} else if (quality_sufficient) {
-				fuseVelocity(aid_src);
+				fuseEvVelocity(aid_src, ev_sample);
 
 			} else {
 				aid_src.innovation_rejected = true;
@@ -177,24 +187,27 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					// Data seems good, attempt a reset
 					_information_events.flags.reset_vel_to_vision = true;
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-					resetVelocityTo(measurement, measurement_var);
+					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 					resetAidSourceStatusZeroInnovation(aid_src);
 
 					if (_control_status.flags.in_air) {
 						_nb_ev_vel_reset_available--;
 					}
 
-				} else if (starting_conditions_passing) {
-					// Data seems good, but previous reset did not fix the issue
-					// something else must be wrong, declare the sensor faulty and stop the fusion
-					//_control_status.flags.ev_vel_fault = true;
-					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-					stopEvVelFusion();
-
 				} else {
-					// A reset did not fix the issue but all the starting checks are not passing
-					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					// differ warning message based on whether the starting conditions are passing
+					if (starting_conditions_passing) {
+						// Data seems good, but previous reset did not fix the issue
+						// something else must be wrong, declare the sensor faulty and stop the fusion
+						//_control_status.flags.ev_vel_fault = true;
+						ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+
+					} else {
+						// A reset did not fix the issue but all the starting checks are not passing
+						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					}
+
 					stopEvVelFusion();
 				}
 			}
@@ -211,15 +224,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
 				ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME,
 					 (double)measurement(0), (double)measurement(1), (double)measurement(2));
-
 				_information_events.flags.reset_vel_to_vision = true;
-
-				resetVelocityTo(measurement, measurement_var);
+				resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 				resetAidSourceStatusZeroInnovation(aid_src);
 
 				_control_status.flags.ev_vel = true;
 
-			} else if (fuseVelocity(aid_src)) {
+			} else if (fuseEvVelocity(aid_src, ev_sample)) {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
 				_control_status.flags.ev_vel = true;
 			}
@@ -232,6 +243,63 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	}
 }
 
+bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample)
+{
+	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+
+		VectorState H;
+		estimator_aid_source1d_s current_aid_src;
+		const auto state_vector = _state.vector();
+
+		for (uint8_t index = 0; index <= 2; index++) {
+			current_aid_src.timestamp_sample = aid_src.timestamp_sample;
+
+			if (index == 0) {
+				sym::ComputeEvBodyVelHx(state_vector, &H);
+
+			} else if (index == 1) {
+				sym::ComputeEvBodyVelHy(state_vector, &H);
+
+			} else {
+				sym::ComputeEvBodyVelHz(state_vector, &H);
+			}
+
+			const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index];
+			const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0);
+
+			updateAidSourceStatus(current_aid_src,
+					      ev_sample.time_us,				// sample timestamp
+					      aid_src.observation[index],			// observation
+					      aid_src.observation_variance[index],		// observation variance
+					      innov,						// innovation
+					      innov_var,    					// innovation variance
+					      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
+
+			if (!current_aid_src.innovation_rejected) {
+				fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H);
+
+			}
+
+			aid_src.innovation[index] = current_aid_src.innovation;
+			aid_src.innovation_variance[index] = current_aid_src.innovation_variance;
+			aid_src.test_ratio[index] = current_aid_src.test_ratio;
+			aid_src.fused = current_aid_src.fused;
+			aid_src.innovation_rejected |= current_aid_src.innovation_rejected;
+
+			if (aid_src.fused) {
+				aid_src.time_last_fuse = _time_delayed_us;
+			}
+
+		}
+
+		aid_src.timestamp_sample = current_aid_src.timestamp_sample;
+		return !aid_src.innovation_rejected;
+
+	} else {
+		return fuseVelocity(aid_src);
+	}
+}
+
 void Ekf::stopEvVelFusion()
 {
 	if (_control_status.flags.ev_vel) {
@@ -239,3 +307,23 @@ void Ekf::stopEvVelFusion()
 		_control_status.flags.ev_vel = false;
 	}
 }
+
+void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var,
+			    const VelocityFrame &vel_frame)
+{
+	if (vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+		resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame);
+
+	} else {
+		resetVelocityTo(measurement, measurement_var);
+	}
+
+}
+
+Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var)
+{
+	// rotate the covariance matrix into the EKF frame
+	const matrix::SquareMatrix<float, 3> R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose();
+	return R_cov.diag();
+}

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -75,15 +75,13 @@ void Ekf::controlFakePosFusion()
 					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 					 innov_gate);                                        // innovation gate
 
-		const bool continuing_conditions_passing = !isHorizontalAidingActive()
+		const bool enable_conditions_passing = !isHorizontalAidingActive()
 				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
+				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
 				&& _horizontal_deadreckon_time_exceeded;
 
 		if (_control_status.flags.fake_pos) {
-			if (continuing_conditions_passing) {
+			if (enable_conditions_passing) {
 
 				// always protect against extreme values that could result in a NaN
 				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
@@ -104,7 +102,7 @@ void Ekf::controlFakePosFusion()
 			}
 
 		} else {
-			if (starting_conditions_passing) {
+			if (enable_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
 				resetFakePosFusion();

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -64,10 +64,16 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 			updateSideslip(_aid_src_sideslip);
 			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
 
-			if (!fuseSideslip(_aid_src_sideslip) && !_control_status.flags.wind) {
+			// If starting wind state estimation, reset the wind states and covariances before fusing any data
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the timeout timers to prevent repeated resets
+				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
 				resetWindToZero();
 			}
-			_control_status.flags.wind = true;
+
+			fuseSideslip(_aid_src_sideslip);
 		}
 	}
 }
@@ -90,10 +96,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 				 math::max(_params.beta_innov_gate, 1.f)); // innovation gate
 }
 
-bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
+void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 {
 	if (sideslip.innovation_rejected) {
-		return false;
+		return;
 	}
 
 	// determine if we need the sideslip fusion to correct states other than wind
@@ -119,7 +125,7 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 
 		ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
 
-		return false;
+		return;
 	}
 
 	_fault_status.flags.bad_sideslip = false;
@@ -143,5 +149,4 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	if (is_fused) {
 		sideslip.time_last_fuse = _time_delayed_us;
 	}
-	return is_fused;
 }

src/modules/ekf2/EKF/common.h

@@ -637,7 +637,6 @@ union information_event_status_u {
 		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
 		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
 		bool reset_pos_to_ext_obs       : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
-		bool reset_wind_to_ext_obs 	: 1; ///< 18 - true when wind states were reset to an external observation
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/covariance.cpp

@@ -195,14 +195,15 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 
 #if defined(CONFIG_EKF2_WIND)
-	// wind vel: add process noise
-	float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f,
-				    1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
-	float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
+	if (_control_status.flags.wind) {
+		// wind vel: add process noise
+		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
+		float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
 
-	for (unsigned index = 0; index < State::wind_vel.dof; index++) {
-		const unsigned i = State::wind_vel.idx + index;
-		P(i, i) = fminf(P(i, i) + wind_vel_process_noise, _params.initial_wind_uncertainty);
+		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
+			const unsigned i = State::wind_vel.idx + index;
+			P(i, i) += wind_vel_process_noise;
+		}
 	}
 #endif // CONFIG_EKF2_WIND
 
@@ -333,3 +334,11 @@ void Ekf::resetMagCov()
 	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+#if defined(CONFIG_EKF2_WIND)
+void Ekf::resetWindCov()
+{
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
+}
+#endif // CONFIG_EKF2_WIND

src/modules/ekf2/EKF/ekf.cpp

@@ -281,23 +281,66 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
 
-void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
 {
 	if (!_pos_ref.isInitialized()) {
-		return;
+		ECL_WARN("unable to reset global position, position reference not initialized");
+		return false;
 	}
 
-	// apply a first order correction using velocity at the delated time horizon and the delta time
-	timestamp_observation = math::min(_time_latest_us, timestamp_observation);
-	const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation)
-			 * 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;
+	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
 
-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
+	// apply a first order correction using velocity at the delayed time horizon and the delta time
+	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
 
-	resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));
+		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 
-	ECL_INFO("reset position to external observation");
-	_information_events.flags.reset_pos_to_ext_obs = true;
+		float diff_us = 0.f;
+
+		if (_time_delayed_us >= timestamp_observation) {
+			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
+
+		} else {
+			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
+		}
+
+		const float dt_s = diff_us * 1e-6f;
+		pos_corrected += _state.vel.xy() * dt_s;
+	}
+
+	const float obs_var = math::max(sq(accuracy), sq(0.01f));
+
+	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
+	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
+
+	const float sq_gate = sq(5.f); // magic hardcoded gate
+	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
+					sq(innov(1)) / (sq_gate * innov_var(1))};
+
+	const bool innov_rejected = (test_ratio.max() > 1.f);
+
+	if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
+		// when on ground or accuracy chosen to be very low, we hard reset position
+		// this allows the user to still send hard resets at any time
+		ECL_INFO("reset position to external observation");
+		_information_events.flags.reset_pos_to_ext_obs = true;
+
+		resetHorizontalPositionTo(pos_corrected, obs_var);
+		_last_known_pos.xy() = _state.pos.xy();
+		return true;
+
+	} else {
+		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
+		 && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
+		) {
+			ECL_INFO("fused external observation as position measurement");
+			_time_last_hor_pos_fuse = _time_delayed_us;
+			_last_known_pos.xy() = _state.pos.xy();
+			return true;
+		}
+	}
+
+	return false;
 }
 
 void Ekf::updateParameters()

src/modules/ekf2/EKF/ekf.h

@@ -501,17 +501,7 @@ public:
 		return true;
 	}
 
-	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
-
-	/**
-	* @brief Resets the wind states to an external observation
-	*
-	* @param wind_speed The wind speed in m/s
-	* @param wind_direction The azimuth (from true north) from where the wind is flowing from in degrees (0 to 360)
-	* @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s
-	* @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in degrees
-	*/
-	void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy);
+	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
 
 	void updateParameters();
 
@@ -772,7 +762,7 @@ private:
 
 	// fuse synthetic zero sideslip measurement
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
-	bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
+	void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
@@ -931,6 +921,8 @@ private:
 	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
 	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
+	void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
+	Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
 
 	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
 	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
@@ -938,6 +930,12 @@ private:
 	void stopEvHgtFusion();
 	void stopEvVelFusion();
 	void stopEvYawFusion();
+	bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample);
+	void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H)
+	{
+		VectorState Kfusion = P * H / innov_var;
+		aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
+	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_GNSS)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -89,7 +89,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			current_pos_available = true;
 		}
 
-		const float gps_alt_ref_prev = getEkfGlobalOriginAltitude();
+		const float gps_alt_ref_prev = _gps_alt_ref;
 
 		// reinitialize map projection to latitude, longitude, altitude, and reset position
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
@@ -114,30 +114,24 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 		_NED_origin_initialised = true;
 
-		// minimum change in position or height that triggers a reset
-		static constexpr float MIN_RESET_DIST_M = 0.01f;
-
 		if (current_pos_available) {
-			// reset horizontal position
+			// reset horizontal position if we already have a global origin
 			Vector2f position = _pos_ref.project(current_lat, current_lon);
-
-			if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) {
-				resetHorizontalPositionTo(position);
-			}
+			resetHorizontalPositionTo(position);
 		}
 
-		// reset vertical position (if there's any change)
-		if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) {
+		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
 			// determine current z
-			float current_alt = -_state.pos(2) + gps_alt_ref_prev;
-
+			const float z_prev = _state.pos(2);
+			const float current_alt = -z_prev + gps_alt_ref_prev;
 #if defined(CONFIG_EKF2_GNSS)
 			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
 #endif // CONFIG_EKF2_GNSS
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
-
+			ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
+				  (double)_state.pos(2));
 #if defined(CONFIG_EKF2_GNSS)
-			// preserve GPS height bias
+			// adjust existing GPS height bias
 			_gps_hgt_b_est.setBias(gps_hgt_bias);
 #endif // CONFIG_EKF2_GNSS
 		}
@@ -574,44 +568,91 @@ void Ekf::updateDeadReckoningStatus()
 
 void Ekf::updateHorizontalDeadReckoningstatus()
 {
-	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
-				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
+	bool inertial_dead_reckoning = true;
+	bool aiding_expected_in_air = false;
+
+	// velocity aiding active
+	if ((_control_status.flags.gps || _control_status.flags.ev_vel)
+	&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+	}
+
+	// position aiding active
+	if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
+	&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+	}
 
-	bool optFlowAiding = false;
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+	// optical flow active
+	if (_control_status.flags.opt_flow
+	&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+
+	} else {
+		if (!_control_status.flags.in_air && (_params.flow_ctrl == 1)
+		&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
+		) {
+			// currently landed, but optical flow aiding should be possible once in air
+			aiding_expected_in_air = true;
+		}
+	}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	bool airDataAiding = false;
-
 #if defined(CONFIG_EKF2_AIRSPEED)
-	airDataAiding = _control_status.flags.wind &&
-				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
-				   isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
+	// air data aiding active
+	if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
+	&& (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
+	) {
+		// wind_dead_reckoning: no other aiding but air data
+		_control_status.flags.wind_dead_reckoning = inertial_dead_reckoning;
 
-	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
-#else
-	_control_status.flags.wind_dead_reckoning = false;
+		// air data aiding is active, we're not inertial dead reckoning
+		inertial_dead_reckoning = false;
+
+	} else {
+		_control_status.flags.wind_dead_reckoning = false;
+
+		if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
+		&& (_params.beta_fusion_enabled == 1)
+		&& (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
+		) {
+			// currently landed, but air data aiding should be possible once in air
+			aiding_expected_in_air = true;
+		}
+	}
 #endif // CONFIG_EKF2_AIRSPEED
 
-	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
-
-	if (!_control_status.flags.inertial_dead_reckoning) {
-		if (_time_delayed_us > _params.no_aid_timeout_max) {
-			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
+	// zero velocity update
+	if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) {
+		// only respect as a valid aiding source now if we expect to have another valid source once in air
+		if (aiding_expected_in_air) {
+			inertial_dead_reckoning = false;
 		}
 	}
 
-	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
-	bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
+	if (inertial_dead_reckoning) {
+		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
+			// deadreckon time exceeded
+			if (!_horizontal_deadreckon_time_exceeded) {
+				ECL_WARN("horizontal dead reckon time exceeded");
+				_horizontal_deadreckon_time_exceeded = true;
+			}
+		}
+
+	} else {
+		if (_time_delayed_us > _params.no_aid_timeout_max) {
+			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
+		}
+
+		_horizontal_deadreckon_time_exceeded = false;
 
-	if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
-		// deadreckon time now exceeded
-		ECL_WARN("dead reckon time exceeded");
 	}
 
-	_horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded;
+	_control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning;
 }
 
 void Ekf::updateVerticalDeadReckoningStatus()
@@ -683,6 +724,30 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_WIND)
+void Ekf::resetWind()
+{
+#if defined(CONFIG_EKF2_AIRSPEED)
+	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
+		resetWindUsingAirspeed(_airspeed_sample_delayed);
+		return;
+	}
+#endif // CONFIG_EKF2_AIRSPEED
+
+	resetWindToZero();
+}
+
+void Ekf::resetWindToZero()
+{
+	ECL_INFO("reset wind to zero");
+
+	// If we don't have an airspeed measurement, then assume the wind is zero
+	_state.wind_vel.setZero();
+
+	resetWindCov();
+}
+
+#endif // CONFIG_EKF2_WIND
 
 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 {

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -298,10 +298,16 @@ def compute_wind_init_and_cov_from_airspeed(
     # Initialise wind states assuming horizontal flight
     sideslip = sf.Symbol("beta")
     wind = sf.V2(v_local[0] - airspeed * sf.cos(heading + sideslip), v_local[1] - airspeed * sf.sin(heading + sideslip))
-    H = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
+    J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
 
-    P = sf.Matrix.diag([v_var[0], v_var[1], heading_var, sideslip_var, airspeed_var])
-    P = H * P * H.T
+    R = sf.M55()
+    R[0,0] = v_var[0]
+    R[1,1] = v_var[1]
+    R[2,2] = heading_var
+    R[3,3] = sideslip_var
+    R[4,4] = airspeed_var
+
+    P = J * R * J.T
 
     # Assume zero sideslip
     P = P.subs({sideslip: 0.0})
@@ -356,6 +362,40 @@ def compute_sideslip_h_and_k(
 
     return (H.T, K)
 
+def predict_vel_body(
+        state: VState
+) -> (sf.V3):
+    vel = state["vel"]
+    R_to_body = state["quat_nominal"].inverse()
+    return R_to_body * vel
+
+def compute_ev_body_vel_hx(
+        state: VState,
+) -> (VTangent):
+
+    state = vstate_to_state(state)
+    meas_pred = predict_vel_body(state)
+    Hx = jacobian_chain_rule(meas_pred[0], state)
+    return (Hx.T)
+
+def compute_ev_body_vel_hy(
+        state: VState,
+) -> (VTangent):
+
+    state = vstate_to_state(state)
+    meas_pred = predict_vel_body(state)[1]
+    Hy = jacobian_chain_rule(meas_pred, state)
+    return (Hy.T)
+
+def compute_ev_body_vel_hz(
+        state: VState,
+) -> (VTangent):
+
+    state = vstate_to_state(state)
+    meas_pred = predict_vel_body(state)[2]
+    Hz = jacobian_chain_rule(meas_pred, state)
+    return (Hz.T)
+
 def predict_mag_body(state) -> sf.V3:
     mag_field_earth = state["mag_I"]
     mag_bias_body = state["mag_B"]
@@ -691,5 +731,8 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas
 generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
 generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
 generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
+generate_px4_function(compute_ev_body_vel_hx, output_names=["H"])
+generate_px4_function(compute_ev_body_vel_hy, output_names=["H"])
+generate_px4_function(compute_ev_body_vel_hz, output_names=["H"])
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h

@@ -0,0 +1,63 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_ev_body_vel_hx
+ *
+ * Args:
+ *     state: Matrix25_1
+ *
+ * Outputs:
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeEvBodyVelHx(const matrix::Matrix<Scalar, 25, 1>& state,
+                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 60
+
+  // Input arrays
+
+  // Intermediate terms (13)
+  const Scalar _tmp0 = 2 * state(5, 0);
+  const Scalar _tmp1 = 2 * state(6, 0);
+  const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0);
+  const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0);
+  const Scalar _tmp4 =
+      (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0);
+  const Scalar _tmp5 = 4 * state(4, 0);
+  const Scalar _tmp6 = 2 * state(1, 0);
+  const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0);
+  const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7;
+  const Scalar _tmp9 = 2 * state(0, 0);
+  const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0);
+  const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10;
+  const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2;
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0);
+    _h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0);
+    _h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0);
+    _h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
+    _h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0);
+    _h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h

@@ -0,0 +1,67 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_ev_body_vel_hy
+ *
+ * Args:
+ *     state: Matrix25_1
+ *
+ * Outputs:
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeEvBodyVelHy(const matrix::Matrix<Scalar, 25, 1>& state,
+                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 64
+
+  // Input arrays
+
+  // Intermediate terms (9)
+  const Scalar _tmp0 = 2 * state(4, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 =
+      -Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0);
+  const Scalar _tmp3 = 2 * state(3, 0);
+  const Scalar _tmp4 =
+      (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0);
+  const Scalar _tmp5 = 4 * state(5, 0);
+  const Scalar _tmp6 = 2 * state(6, 0);
+  const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) -
+                       Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) +
+                       (Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0);
+  const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) -
+                       Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) +
+                       (Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0);
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) =
+        -_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0);
+    _h(1, 0) =
+        -_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0);
+    _h(2, 0) =
+        -_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0);
+    _h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0);
+    _h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
+    _h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h

@@ -0,0 +1,63 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_ev_body_vel_hz
+ *
+ * Args:
+ *     state: Matrix25_1
+ *
+ * Outputs:
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeEvBodyVelHz(const matrix::Matrix<Scalar, 25, 1>& state,
+                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 60
+
+  // Input arrays
+
+  // Intermediate terms (13)
+  const Scalar _tmp0 = 2 * state(4, 0);
+  const Scalar _tmp1 = 2 * state(5, 0);
+  const Scalar _tmp2 =
+      (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0);
+  const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0);
+  const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3;
+  const Scalar _tmp5 = 4 * state(6, 0);
+  const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0);
+  const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6;
+  const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0);
+  const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0);
+  const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8;
+  const Scalar _tmp11 = 2 * state(2, 0);
+  const Scalar _tmp12 = 2 * state(1, 0);
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9;
+    _h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9;
+    _h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0);
+    _h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0);
+    _h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0);
+    _h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/wind.cpp

@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 wind.cpp
- * Helper functions for wind states
- */
-
-#include "ekf.h"
-
-void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy)
-{
-	const float wind_speed_constrained = math::max(wind_speed, 0.0f);
-
-	// wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO
-	const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F);
-	Vector2f wind = wind_speed_constrained * Vector2f(cosf(wind_direction_rad), sinf(wind_direction_rad));
-
-	ECL_INFO("reset wind states to external observation");
-	_information_events.flags.reset_wind_to_ext_obs = true;
-
-	Vector2f innov = _state.wind_vel - wind;
-	float R = sq(0.1f);
-	Vector2f innov_var{P(State::wind_vel.idx, State::wind_vel.idx) + R, P(State::wind_vel.idx + 1, State::wind_vel.idx + 1) + R};
-	const bool control_status_wind_prev = _control_status.flags.wind;
-	_control_status.flags.wind = true;
-	fuseDirectStateMeasurement(innov(0), innov_var(0), R, State::wind_vel.idx);
-	fuseDirectStateMeasurement(innov(1), innov_var(1), R, State::wind_vel.idx + 1);
-	_control_status.flags.wind = control_status_wind_prev;
-
-	// reset the horizontal velocity variances to allow the velocity states to be pulled towards
-	// a solution that is aligned with the newly set wind estimates
-	static constexpr float hor_vel_var = 25.0f;
-	P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var);
-}
-
-void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)
-{
-	_state.wind_vel = wind;
-
-	if (PX4_ISFINITE(wind_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, math::max(sq(_params.initial_wind_uncertainty), wind_var(0)));
-	}
-
-	if (PX4_ISFINITE(wind_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, math::max(sq(_params.initial_wind_uncertainty), wind_var(1)));
-	}
-}
-
-void Ekf::resetWindCov()
-{
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
-}
-
-void Ekf::resetWind()
-{
-#if defined(CONFIG_EKF2_AIRSPEED)
-	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
-		resetWindUsingAirspeed(_airspeed_sample_delayed);
-		return;
-	}
-#endif // CONFIG_EKF2_AIRSPEED
-
-	resetWindToZero();
-}
-
-void Ekf::resetWindToZero()
-{
-	ECL_INFO("reset wind to zero");
-
-	// If we don't have an airspeed measurement, then assume the wind is zero
-	_state.wind_vel.setZero();
-
-	resetWindCov();
-}

src/modules/ekf2/EKF2.cpp

@@ -497,6 +497,12 @@ void EKF2::Run()
 		vehicle_command_s vehicle_command;
 
 		if (_vehicle_command_sub.update(&vehicle_command)) {
+
+			vehicle_command_ack_s command_ack{};
+			command_ack.command = vehicle_command.command;
+			command_ack.target_system = vehicle_command.source_system;
+			command_ack.target_component = vehicle_command.source_component;
+
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
 				double latitude = vehicle_command.param5;
 				double longitude = vehicle_command.param6;
@@ -509,15 +515,24 @@ void EKF2::Run()
 					PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
 						 _instance, latitude, longitude, static_cast<double>(altitude));
 
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
 				} else {
 					PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
 						_instance, latitude, longitude, static_cast<double>(altitude));
-				}
-			}
 
-			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
-				if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) &&
-				    PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) {
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+				}
+
+				command_ack.timestamp = hrt_absolute_time();
+				_vehicle_command_ack_pub.publish(command_ack);
+
+			} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
+
+				if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning
+				     || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2)
+				    && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)
+				   ) {
 
 					const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,
 										kMaxDelaySecondsExternalPosMeasurement);
@@ -530,16 +545,21 @@ void EKF2::Run()
 					}
 
 					// TODO add check for lat and long validity
-					_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
-							accuracy, timestamp_observation);
-				}
-			}
+					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
+							accuracy, timestamp_observation)
+					   ) {
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
-			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
-				if (_ekf.control_status_flags().wind_dead_reckoning || !_ekf.control_status_flags().in_air) {
-					_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
-									    vehicle_command.param4);
+					} else {
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+					}
+
+				} else {
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; // TODO: expand
 				}
+
+				command_ack.timestamp = hrt_absolute_time();
+				_vehicle_command_ack_pub.publish(command_ack);
 			}
 		}
 	}

src/modules/ekf2/EKF2.hpp

@@ -78,6 +78,7 @@
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_land_detected.h>
@@ -387,9 +388,11 @@ private:
 
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
 

src/modules/ekf2/test/test_EKF_externalVision.cpp

@@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
 
 TEST_F(EkfExternalVisionTest, velocityFrameBody)
 {
-	// GIVEN: Drone is turned 90 degrees
-	const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
-	_sensor_simulator.simulateOrientation(quat_sim);
+	_ekf_wrapper.setMagFuseTypeNone();
 	_sensor_simulator.runSeconds(_tilt_align_time);
-
 	_ekf->set_vehicle_at_rest(false);
 
-	// Without any measurement x and y velocity variance are close
-	const Vector3f velVar_init = _ekf->getVelocityVariance();
-	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
-
-	// WHEN: measurement is given in BODY-FRAME and
-	//       x variance is bigger than y variance
-	_sensor_simulator._vio.setVelocityFrameToBody();
+	float yaw_var0 = _ekf->getYawVar();
 
 	const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
 	const Vector3f vel_body(1.0f, 0.0f, 0.0f);
+	_sensor_simulator._vio.setVelocityFrameToBody();
 	_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
 	_sensor_simulator._vio.setVelocity(vel_body);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(4);
 
-	// THEN: As the drone is turned 90 degrees, velocity variance
-	//       along local y axis is expected to be bigger
-	const Vector3f velVar_new = _ekf->getVelocityVariance();
-	EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f);
-
-	const Vector3f vel_earth_est = _ekf->getVelocity();
-	EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f);
-	EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f);
+	const Vector3f vel_var = _ekf->getVelocityVariance();
+	EXPECT_TRUE(yaw_var0 < _ekf->getYawVar());
+	EXPECT_TRUE(vel_var(1) < vel_var(0));
 }
 
 TEST_F(EkfExternalVisionTest, velocityFrameLocal)

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -105,7 +105,7 @@ TEST_F(EkfGpsTest, gpsFixLoss)
 	_sensor_simulator._gps.setFixType(0);
 
 	// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated
-	_sensor_simulator.runSeconds(5);
+	_sensor_simulator.runSeconds(6);
 	EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
 	EXPECT_FALSE(_ekf->local_position_is_valid());