Merge branch 'main' of github.com:PX4/PX4-Autopilot into pr-fw_ctrl_api

src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 	DEPENDS
 		drivers_rangefinder
 		px4_work_queue
+		CollisionPrevention
 	MODULE_CONFIG
 		module.yaml
 	)

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -33,19 +33,18 @@
 
 #include "lightware_sf45_serial.hpp"
 
-#include <inttypes.h>
 #include <fcntl.h>
+#include <float.h>
+#include <inttypes.h>
 #include <termios.h>
+
 #include <lib/crc/crc.h>
 #include <lib/mathlib/mathlib.h>
-
-#include <float.h>
+#include <matrix/matrix/math.hpp>
+#include <ObstacleMath.hpp>
 
 using namespace time_literals;
 
-/* Configuration Constants */
-
-
 SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@@ -93,6 +92,11 @@ int SF45LaserSerial::init()
 	param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
 
+	// set the sensor orientation
+	const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+				    (_yaw_cfg));
+	_obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle));
+
 	start();
 	return PX4_OK;
 }
@@ -136,8 +140,6 @@ int SF45LaserSerial::measure()
 
 int SF45LaserSerial::collect()
 {
-	float distance_m = -1.0f;
-
 	if (_sensor_state == STATE_UNINIT) {
 
 		perf_begin(_sample_perf);
@@ -196,8 +198,7 @@ int SF45LaserSerial::collect()
 		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
 
 		if (_crc_valid) {
-			sf45_process_replies(&distance_m);
-			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
+			sf45_process_replies();
 			perf_end(_sample_perf);
 			return PX4_OK;
 		}
@@ -592,13 +593,12 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 	}
 }
 
-void SF45LaserSerial::sf45_process_replies(float *distance_m)
+void SF45LaserSerial::sf45_process_replies()
 {
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
-			int16_t scaled_yaw = 0;
 
 			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
 			if (raw_yaw > 32000) {
@@ -611,49 +611,41 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			}
 
 			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
-			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
+			float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
-			switch (_yaw_cfg) {
-			case ROTATION_FORWARD_FACING:
-				break;
-
-			case ROTATION_BACKWARD_FACING:
-				if (scaled_yaw > 180) {
-					scaled_yaw = scaled_yaw - 180;
-
-				} else {
-					scaled_yaw = scaled_yaw + 180; // rotation facing aft
-				}
-
-				break;
-
-			case ROTATION_RIGHT_FACING:
-				scaled_yaw = scaled_yaw + 90; // rotation facing right
-				break;
-
-			case ROTATION_LEFT_FACING:
-				scaled_yaw = scaled_yaw - 90; // rotation facing left
-				break;
-
-			default:
-				break;
-			}
+			// Adjust for sensor orientation
+			scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
 
 			// Convert to meters for the debug message
-			*distance_m = raw_distance * SF45_SCALE_FACTOR;
+			float distance_m = raw_distance * SF45_SCALE_FACTOR;
 			_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
 
 			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
 
 			if (current_bin != _previous_bin) {
-				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
-					  (double)*distance_m);
+				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
+					  (double)distance_m);
+
+				if (_vehicle_attitude_sub.updated()) {
+					vehicle_attitude_s vehicle_attitude;
+
+					if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+						_vehicle_attitude = matrix::Quatf(vehicle_attitude.q);
+					}
+				}
+
+				float current_bin_dist = static_cast<float>(_current_bin_dist);
+				float scaled_yaw_rad = math::radians(static_cast<float>(scaled_yaw));
+				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude);
+				_current_bin_dist = static_cast<uint16_t>(current_bin_dist);
 
 				if (_current_bin_dist > _obstacle_distance.max_distance) {
 					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
 				}
 
 				hrt_abstime now = hrt_absolute_time();
+
+				_obstacle_distance.distances[current_bin] = _current_bin_dist;
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
 
 				_publish_obstacle_msg(now);
@@ -691,47 +683,31 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 {
 	// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
 	// in this case we assume the measurement to be valid for all bins between the previous and the current bin.
-	uint8_t start;
-	uint8_t end;
 
-	if (abs(current_bin - previous_bin) > BIN_COUNT / 4) {
-		// wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins
-		// THis is simplyfied as we are not considering the scaning direction
-		start = math::max(previous_bin, current_bin);
-		end = math::min(previous_bin, current_bin);
+	// Shift bin indices such that we can never have the wrap-around case.
+	const float    fov_offset_angle    = 360.0f - SF45_FIELDOF_VIEW / 2.f;
+	const uint16_t current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  _obstacle_distance.increment,
+					     fov_offset_angle);
+	const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment,
+					     fov_offset_angle);
 
-	} else if (previous_bin < current_bin) {	// Scanning clockwise
-		start = previous_bin + 1;
-		end = current_bin;
+	const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1;
+	const uint16_t end   = math::max(current_bin_offset, previous_bin_offset);
 
-	} else { 					// scanning counter-clockwise
-		start = current_bin;
-		end = previous_bin - 1;
-	}
-
-	if (start <= end) {
-		for (uint8_t i = start; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-	} else { // wrap-around case
-		for (uint8_t i = start; i < BIN_COUNT; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-		for (uint8_t i = 0; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
+	// populate the missed bins with the measurement
+	for (uint16_t i = start; i < end; i++) {
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle);
+		_obstacle_distance.distances[bin_index] = measurement;
+		_data_timestamps[bin_index] = now;
 	}
 }
+
+
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;
 	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
+	mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
 
 	return mapped_sector;
 }

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -49,7 +49,9 @@
 #include <lib/perf/perf_counter.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 #include "sf45_commands.h"
 
@@ -92,7 +94,7 @@ public:
 	void				sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
 	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
 	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void				sf45_process_replies(float *data);
+	void				sf45_process_replies();
 	uint8_t				sf45_convert_angle(const int16_t yaw);
 	float				sf45_wrap_360(float f);
 
@@ -103,6 +105,7 @@ private:
 				obstacle_distance_s::distances[0]);
 	static constexpr uint64_t 	SF45_MEAS_TIMEOUT{100_ms};
 	static constexpr float 		SF45_SCALE_FACTOR = 0.01f;
+	static constexpr float		SF45_FIELDOF_VIEW = 320.f; // degrees
 
 	void				start();
 	void				stop();
@@ -113,6 +116,7 @@ private:
 
 	void 				_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
 	void 				_publish_obstacle_msg(hrt_abstime now);
+	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];
 
 
@@ -141,6 +145,7 @@ private:
 	int32_t				_orient_cfg{0};
 	uint8_t				_previous_bin{0};
 	uint16_t			_current_bin_dist{UINT16_MAX};
+	matrix::Quatf			_vehicle_attitude{};
 
 	// end of SF45/B data members
 

src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp

@@ -102,7 +102,7 @@ static int usage()
 
 Serial bus driver for the Aerotenna uLanding radar.
 
-Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/ulanding_radar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/ulanding_radar.html
 
 ### Examples
 

src/drivers/drv_dshot.h

@@ -88,7 +88,7 @@ typedef enum {
  * @param channel_mask		Bitmask of channels (LSB = channel 0) to enable.
  *				This allows some of the channels to remain configured
  *				as GPIOs or as another function. Already used channels/timers will not be configured as DShot
- * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
+ * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600
  * @return <0 on error, the initialized channels mask.
  */
 __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot);

src/drivers/dshot/DShot.cpp

@@ -125,9 +125,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			} else if (tim_config == -3) {
 				dshot_frequency_request = DSHOT600;
 
-			} else if (tim_config == -2) {
-				dshot_frequency_request = DSHOT1200;
-
 			} else {
 				_output_mask &= ~channels; // don't use for dshot
 			}
@@ -824,7 +821,7 @@ On startup, the module tries to occupy all available pins for DShot output.
 It skips all pins already in use (e.g. by a camera trigger module).
 
 It supports:
-- DShot150, DShot300, DShot600, DShot1200
+- DShot150, DShot300, DShot600
 - telemetry via separate UART and publishing as esc_status message
 - sending DShot commands via CLI
 

src/drivers/dshot/DShot.h

@@ -52,7 +52,6 @@ using namespace time_literals;
 static constexpr unsigned int DSHOT150  =  150000u;
 static constexpr unsigned int DSHOT300  =  300000u;
 static constexpr unsigned int DSHOT600  =  600000u;
-static constexpr unsigned int DSHOT1200 = 1200000u;
 
 static constexpr int DSHOT_DISARM_VALUE = 0;
 static constexpr int DSHOT_MIN_THROTTLE = 1;
@@ -107,7 +106,6 @@ private:
 		DShot150  = 150,
 		DShot300  = 300,
 		DShot600  = 600,
-		DShot1200 = 1200,
 	};
 
 	struct Command {

src/drivers/ins/vectornav/VectorNav.cpp

@@ -328,7 +328,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			local_position.vxy_max = INFINITY;
 			local_position.vz_max = INFINITY;
 			local_position.hagl_min = INFINITY;
-			local_position.hagl_max = INFINITY;
+			local_position.hagl_max_z = INFINITY;
+			local_position.hagl_max_xy = INFINITY;
 
 			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();

src/drivers/power_monitor/ina238/ina238.cpp

@@ -160,6 +160,8 @@ int INA238::Reset()
 
 	int ret = PX4_ERROR;
 
+	_retries = 3;
+
 	if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) {
 		return ret;
 	}
@@ -231,6 +233,16 @@ int INA238::collect()
 	success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
 	success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);
 
+	if (success) {
+		_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
+		_battery.updateCurrent(static_cast<float>(current * _current_lsb));
+		_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
+
+		_battery.setConnected(success);
+
+		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
+	}
+
 	if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 		// check configuration registers periodically or immediately following any failure
 		if (RegisterCheck(_register_cfg[_checked_register])) {
@@ -242,26 +254,21 @@ int INA238::collect()
 			PX4_DEBUG("register check failed");
 			perf_count(_bad_register_perf);
 			success = false;
+
+			_battery.setConnected(success);
+
+			_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 		}
 	}
 
-	if (!success) {
-		PX4_DEBUG("error reading from sensor");
-		bus_voltage = current = 0;
-	}
-
-	_battery.setConnected(success);
-	_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
-	_battery.updateCurrent(static_cast<float>(current * _current_lsb));
-	_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
-	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
-
 	perf_end(_sample_perf);
 
 	if (success) {
 		return PX4_OK;
 
 	} else {
+		PX4_DEBUG("error reading from sensor");
+
 		return PX4_ERROR;
 	}
 }

src/drivers/pwm_out/module.yaml

@@ -23,7 +23,6 @@ actuator_output:
             -5: DShot150
             -4: DShot300
             -3: DShot600
-            -2: DShot1200
             -1: OneShot
             50: PWM 50 Hz
             100: PWM 100 Hz

src/drivers/uavcan/uavcan_main.cpp

@@ -705,11 +705,12 @@ UavcanNode::Run()
 
 		if (can_init_res < 0) {
 			PX4_ERR("CAN driver init failed %i", can_init_res);
+
+		} else {
+			_instance->init(node_id, can->driver.updateEvent());
+
+			_node_init = true;
 		}
-
-		_instance->init(node_id, can->driver.updateEvent());
-
-		_node_init = true;
 	}
 
 	pthread_mutex_lock(&_node_mutex);
@@ -1072,8 +1073,6 @@ void UavcanMixingInterfaceServo::Run()
 void
 UavcanNode::print_info()
 {
-	(void)pthread_mutex_lock(&_node_mutex);
-
 	// Memory status
 	printf("Pool allocator status:\n");
 	printf("\tCapacity hard/soft: %" PRIu16 "/%" PRIu16 " blocks\n",
@@ -1111,15 +1110,29 @@ UavcanNode::print_info()
 	printf("\n");
 
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-	printf("ESC outputs:\n");
-	_mixing_interface_esc.mixingOutput().printStatus();
 
-	printf("Servo outputs:\n");
-	_mixing_interface_servo.mixingOutput().printStatus();
+	// Print esc status if at least one channel is enabled
+	for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+		if (_mixing_interface_esc.mixingOutput().isFunctionSet(i)) {
+			printf("ESC outputs:\n");
+			_mixing_interface_esc.mixingOutput().printStatus();
+			printf("\n");
+			break;
+		}
+	}
+
+	// Print servo status if at least one channel is enabled
+	for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+		if (_mixing_interface_servo.mixingOutput().isFunctionSet(i)) {
+			printf("Servo outputs:\n");
+			_mixing_interface_servo.mixingOutput().printStatus();
+			printf("\n");
+			break;
+		}
+	}
+
 #endif
 
-	printf("\n");
-
 	// Sensor bridges
 	for (const auto &br : _sensor_bridges) {
 		printf("Sensor '%s':\n", br->get_name());
@@ -1139,8 +1152,6 @@ UavcanNode::print_info()
 
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-
-	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
 void

src/lib/CMakeLists.txt

@@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL)
 add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL)
 add_subdirectory(collision_prevention EXCLUDE_FROM_ALL)
 add_subdirectory(component_information EXCLUDE_FROM_ALL)
+add_subdirectory(control_allocation EXCLUDE_FROM_ALL)
 add_subdirectory(controllib EXCLUDE_FROM_ALL)
 add_subdirectory(conversion EXCLUDE_FROM_ALL)
 add_subdirectory(crc EXCLUDE_FROM_ALL)

src/lib/collision_prevention/CMakeLists.txt

@@ -31,7 +31,13 @@
 #
 ############################################################################
 
-px4_add_library(CollisionPrevention CollisionPrevention.cpp)
+px4_add_library(CollisionPrevention
+	CollisionPrevention.cpp
+	ObstacleMath.cpp
+)
 target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable
+target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(CollisionPrevention PRIVATE mathlib)
 
 px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention)
+px4_add_unit_gtest(SRC ObstacleMathTest.cpp LINKLIBS CollisionPrevention)

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -38,6 +38,7 @@
  */
 
 #include "CollisionPrevention.hpp"
+#include "ObstacleMath.hpp"
 #include <px4_platform_common/events.h>
 
 using namespace matrix;
@@ -400,18 +401,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		int lower_bound = (int)round((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 		int upper_bound = (int)round((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 
-		const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));
-
-		const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
-
-		const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;
-
-		const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
-
-		const float sensor_dist_scale = rotated_sensor_vector.xy().norm();
-
 		if (distance_reading < distance_sensor.max_distance) {
-			distance_reading = distance_reading * sensor_dist_scale;
+			ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude);
 		}
 
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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

src/lib/collision_prevention/ObstacleMath.cpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ObstacleMath.hpp"
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+namespace ObstacleMath
+{
+
+void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle)
+{
+	const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
+	const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
+	const Vector3f forward(1.f, 0.f, 0.f);
+	const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
+
+	float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
+	horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
+	distance *= horizontal_projection_scale;
+}
+
+int get_bin_at_angle(float bin_width, float angle)
+{
+	int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width);
+	return wrap_bin(bin_at_angle, 360 / bin_width);
+}
+
+int get_offset_bin_index(int bin, float bin_width, float angle_offset)
+{
+	int offset = get_bin_at_angle(bin_width, angle_offset);
+	return wrap_bin(bin - offset, 360 / bin_width);
+}
+
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
+{
+	float offset = 0.0f;
+
+	switch (orientation) {
+	case SensorOrientation::ROTATION_YAW_0:
+		offset = 0.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_45:
+		offset = M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_90:
+		offset = M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_135:
+		offset = 3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_180:
+		offset = M_PI_F;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_225:
+		offset = -3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_270:
+		offset = -M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_315:
+		offset = -M_PI_F / 4.0f;
+		break;
+	}
+
+	return offset;
+}
+
+int wrap_bin(int bin, int bin_count)
+{
+	return (bin + bin_count) % bin_count;
+}
+
+} // ObstacleMath

src/lib/collision_prevention/ObstacleMath.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <matrix/math.hpp>
+
+namespace ObstacleMath
+{
+
+enum SensorOrientation {
+	ROTATION_YAW_0   = 0,     // MAV_SENSOR_ROTATION_NONE
+	ROTATION_YAW_45  = 1,	  // MAV_SENSOR_ROTATION_YAW_45
+	ROTATION_YAW_90  = 2,	  // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_YAW_135 = 3,	  // MAV_SENSOR_ROTATION_YAW_135
+	ROTATION_YAW_180 = 4,	  // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_YAW_225 = 5,	  // MAV_SENSOR_ROTATION_YAW_225
+	ROTATION_YAW_270 = 6,	  // MAV_SENSOR_ROTATION_YAW_270
+	ROTATION_YAW_315 = 7,	  // MAV_SENSOR_ROTATION_YAW_315
+
+	ROTATION_FORWARD_FACING  = 0, // MAV_SENSOR_ROTATION_NONE
+	ROTATION_RIGHT_FACING    = 2, // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_LEFT_FACING     = 6  // MAV_SENSOR_ROTATION_YAW_270
+};
+
+/**
+ * Converts a sensor orientation to a yaw offset
+ * @param orientation sensor orientation
+ */
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
+
+/**
+ * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
+ * @param distance measurement which is scaled down
+ * @param yaw orientation of the measurement on the body horizontal plane
+ * @param q_world_vehicle vehicle attitude quaternion
+ */
+void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle);
+
+/**
+ * Returns bin index at a given angle from the 0th bin
+ * @param bin_width width of a bin in degrees
+ * @param angle clockwise angle from start bin in degrees
+ */
+int get_bin_at_angle(float bin_width, float angle);
+
+/**
+ * Returns bin index for the current bin after an angle offset
+ * @param bin current bin index
+ * @param bin_width width of a bin in degrees
+ * @param angle_offset clockwise angle offset in degrees
+ */
+int get_offset_bin_index(int bin, float bin_width, float angle_offset);
+
+/**
+ * Wraps a bin index to the range [0, bin_count)
+ * @param bin bin index
+ * @param bin_count number of bins
+ */
+int wrap_bin(int bin, int bin_count);
+
+} // ObstacleMath

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+#include "ObstacleMath.hpp"
+
+using namespace matrix;
+
+TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane)
+{
+	// standard vehicle orientation inputs
+	Quatf vehicle_pitch_up_45(Eulerf(0.0f, M_PI_4_F, 0.0f));
+	Quatf vehicle_roll_right_45(Eulerf(M_PI_4_F, 0.0f, 0.0f));
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	float distance = 1.0f;
+	float sensor_orientation = 0; // radians (forward facing)
+
+	// WHEN: we project the distance onto the horizontal plane
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
+
+	// THEN: the distance should be scaled correctly
+	float expected_scale    = sqrtf(2) / 2;
+	float expected_distance = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = 1.f;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+	sensor_orientation = M_PI_2_F; // radians (right facing)
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = sqrtf(2) / 2;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = 1.f;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+}
+
+TEST(ObstacleMathTest, GetBinAtAngle)
+{
+	float bin_width = 5.0f;
+
+	// GIVEN: a start bin, bin width, and angle
+	float angle = 0.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 0);
+
+	// GIVEN: a start bin, bin width, and angle
+	angle = 90.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 18);
+
+	// GIVEN: a start bin, bin width, and angle
+	angle = -90.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 54);
+
+	// GIVEN: a start bin, bin width, and angle
+	angle = 450.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 18);
+}
+
+
+TEST(ObstacleMathTest, OffsetBinIndex)
+{
+	// In this test, we want to offset the bin index by a negative and positive angle.
+	// We take the output of the first offset and offset it by the same angle in the
+	// opposite direction to return back to the original bin index.
+
+	// GIVEN: a bin index, bin width, and a negative angle offset
+	uint16_t bin = 0;
+	float bin_width = 5.0f;
+	float angle_offset = -120.0f;
+
+	// WHEN: we offset the bin index by the negative angle
+	uint16_t new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
+
+	// THEN: the new bin index should be correctly offset by the wrapped angle
+	EXPECT_EQ(new_bin_index, 24);
+
+	// GIVEN: the output bin index of the previous offset, bin width, and the same angle
+	// offset in positive direction
+	bin = 24;
+	bin_width = 5.0f;
+	angle_offset = 120.0f;
+
+	// WHEN: we offset the bin index by the positive angle
+	new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
+
+	// THEN: the new bin index should return back to the original bin index
+	EXPECT_EQ(new_bin_index, 0);
+}
+
+
+TEST(ObstacleMathTest, WrapBin)
+{
+	// GIVEN: a bin index within bounds and the number of bins
+	int bin = 0;
+	int bin_count = 72;
+
+	// WHEN: we wrap a bin index within the bounds
+	int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should stay 0
+	EXPECT_EQ(wrapped_bin, 0);
+
+	// GIVEN: a bin index that is out of bounds, and the number of bins
+	bin = 73;
+	bin_count = 72;
+
+	// WHEN: we wrap a bin index that is larger than the number of bins
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be wrapped back to the beginning
+	EXPECT_EQ(wrapped_bin, 1);
+
+	// GIVEN: a negative bin index and the number of bins
+	bin = -1;
+	bin_count = 72;
+
+	// WHEN: we wrap a bin index that is negative
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be wrapped back to the end
+	EXPECT_EQ(wrapped_bin, 71);
+}
+
+TEST(ObstacleMathTest, HandleMissedBins)
+{
+	// In this test, the current and previous bin are adjacent to the bins that are outside
+	// the sensor field of view. The missed bins (0,1,6 & 7) should be populated, and no
+	// data should be filled in the bins outside the FOV.
+
+	// GIVEN: measurements, current bin, previous bin, bin width, and field of view offset
+	float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0};
+	int   current_bin     = 2;
+	int   previous_bin    = 5;
+	int   bin_width       = 45.0f;
+	float fov             = 270.0f;
+	float fov_offset      = 360.0f - fov / 2;
+
+	float measurement     = measurements[current_bin];
+
+	// WHEN: we handle missed bins
+	int current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  bin_width, fov_offset);
+	int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset);
+
+	int start = math::min(current_bin_offset, previous_bin_offset) + 1;
+	int end   = math::max(current_bin_offset, previous_bin_offset);
+
+	EXPECT_EQ(start, 1);
+	EXPECT_EQ(end,   5);
+
+	for (uint16_t i = start; i < end; i++) {
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset);
+		measurements[bin_index] = measurement;
+	}
+
+	// THEN: the correct missed bins should be populated with the measurement
+	EXPECT_EQ(measurements[0], 1);
+	EXPECT_EQ(measurements[1], 1);
+	EXPECT_EQ(measurements[2], 1);
+	EXPECT_EQ(measurements[3], 0);
+	EXPECT_EQ(measurements[4], 0);
+	EXPECT_EQ(measurements[5], 2);
+	EXPECT_EQ(measurements[6], 1);
+	EXPECT_EQ(measurements[7], 1);
+}

src/lib/control_allocation/CMakeLists.txt

@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 2025 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.
+#
+############################################################################
+
+add_subdirectory(control_allocation)
+add_subdirectory(actuator_effectiveness)

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -32,7 +32,6 @@
  ****************************************************************************/
 
 #include "ActuatorEffectiveness.hpp"
-#include "../ControlAllocation/ControlAllocation.hpp"
 
 #include <px4_platform_common/log.h>
 
@@ -51,12 +50,12 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m
 		return -1;
 	}
 
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
 	matrix_selection_indexes[totalNumActuators()] = selected_matrix;
 	++num_actuators[(int)type];
 	return num_actuators_matrix[selected_matrix]++;

src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

@@ -34,34 +34,6 @@
 px4_add_library(ActuatorEffectiveness
 	ActuatorEffectiveness.cpp
 	ActuatorEffectiveness.hpp
-	ActuatorEffectivenessUUV.cpp
-	ActuatorEffectivenessUUV.hpp
-	ActuatorEffectivenessControlSurfaces.cpp
-	ActuatorEffectivenessControlSurfaces.hpp
-	ActuatorEffectivenessCustom.cpp
-	ActuatorEffectivenessCustom.hpp
-	ActuatorEffectivenessFixedWing.cpp
-	ActuatorEffectivenessFixedWing.hpp
-	ActuatorEffectivenessHelicopter.cpp
-	ActuatorEffectivenessHelicopter.hpp
-	ActuatorEffectivenessHelicopterCoaxial.cpp
-	ActuatorEffectivenessHelicopterCoaxial.hpp
-	ActuatorEffectivenessMCTilt.cpp
-	ActuatorEffectivenessMCTilt.hpp
-	ActuatorEffectivenessMultirotor.cpp
-	ActuatorEffectivenessMultirotor.hpp
-	ActuatorEffectivenessTilts.cpp
-	ActuatorEffectivenessTilts.hpp
-	ActuatorEffectivenessRotors.cpp
-	ActuatorEffectivenessRotors.hpp
-	ActuatorEffectivenessStandardVTOL.cpp
-	ActuatorEffectivenessStandardVTOL.hpp
-	ActuatorEffectivenessTiltrotorVTOL.cpp
-	ActuatorEffectivenessTiltrotorVTOL.hpp
-	ActuatorEffectivenessTailsitterVTOL.cpp
-	ActuatorEffectivenessTailsitterVTOL.hpp
-	ActuatorEffectivenessRoverAckermann.hpp
-	ActuatorEffectivenessRoverAckermann.cpp
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -69,7 +41,5 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
+		PID
 )
-
-px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
-px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)

src/lib/control_allocation/control_allocation/ControlAllocation.hpp

@@ -71,7 +71,7 @@
 
 #include <matrix/matrix/math.hpp>
 
-#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 class ControlAllocation
 {

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp

@@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix(
 			update_normalization_scale);
 	_mix_update_needed = true;
 	_normalization_needs_update = update_normalization_scale;
+
+	if (_metric_allocation && update_normalization_scale) {
+		// adding #include <px4_platform_common/log.h> + PX4_WARN leads to failed linking on test
+		_normalization_needs_update = false;
+	}
 }
 
 void
@@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
 
-		if (_normalization_needs_update && !_had_actuator_failure) {
-			updateControlAllocationMatrixScale();
-			_normalization_needs_update = false;
+		if (!_metric_allocation) {
+			if (_normalization_needs_update && !_had_actuator_failure) {
+				updateControlAllocationMatrixScale();
+				_normalization_needs_update = false;
+			}
+
+			normalizeControlAllocationMatrix();
 		}
 
-		normalizeControlAllocationMatrix();
 		_mix_update_needed = false;
 	}
 }

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp

@@ -57,11 +57,13 @@ public:
 	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
 				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
 				    bool update_normalization_scale) override;
+	void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; }
 
 protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
 
 	bool _mix_update_needed{false};
+	bool _metric_allocation{false};
 
 	/**
 	 * Recalculate pseudo inverse if required.

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp

@@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase)
 	EXPECT_EQ(actuator_sp, actuator_sp_expected);
 	EXPECT_EQ(control_allocated, control_allocated_expected);
 }
+
+TEST(ControlAllocationMetricTest, AllZeroCase)
+{
+	ControlAllocationPseudoInverse method;
+
+	matrix::Vector<float, 6> control_sp;
+	matrix::Vector<float, 6> control_allocated;
+	matrix::Vector<float, 6> control_allocated_expected;
+	matrix::Matrix<float, 6, 16> effectiveness;
+	matrix::Vector<float, 16> actuator_sp;
+	matrix::Vector<float, 16> actuator_trim;
+	matrix::Vector<float, 16> linearization_point;
+	matrix::Vector<float, 16> actuator_sp_expected;
+
+	method.setMetricAllocation(true);
+	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
+	method.setControlSetpoint(control_sp);
+	method.allocate();
+	actuator_sp = method.getActuatorSetpoint();
+	control_allocated_expected = method.getAllocatedControl();
+
+	EXPECT_EQ(actuator_sp, actuator_sp_expected);
+	EXPECT_EQ(control_allocated, control_allocated_expected);
+}

src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp

@@ -40,17 +40,27 @@
 
 #include <gtest/gtest.h>
 #include <ControlAllocationSequentialDesaturation.hpp>
-#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp>
 
 using namespace matrix;
 
 namespace
 {
+struct RotorGeometryTest {
+	matrix::Vector3f position;
+	matrix::Vector3f axis;
+	float thrust_coef;
+	float moment_ratio;
+};
+
+struct GeometryTest {
+	RotorGeometryTest rotors[ActuatorEffectiveness::NUM_ACTUATORS];
+	int num_rotors{0};
+};
 
 // Makes and returns a Geometry object for a "standard" quad-x quadcopter.
-ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
+GeometryTest make_quad_x_geometry()
 {
-	ActuatorEffectivenessRotors::Geometry geometry = {};
+	GeometryTest geometry = {};
 	geometry.rotors[0].position(0) = 1.0f;
 	geometry.rotors[0].position(1) = 1.0f;
 	geometry.rotors[0].position(2) = 0.0f;
@@ -88,7 +98,6 @@ ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
 	geometry.rotors[3].moment_ratio = -0.05f;
 
 	geometry.num_rotors = 4;
-
 	return geometry;
 }
 
@@ -98,7 +107,48 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
 	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
 	effectiveness.setZero();
 	const auto geometry = make_quad_x_geometry();
-	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+
+	// Minimalistically copied from ActuatorEffectivenessRotors::computeEffectivenessMatrix
+	for (int i = 0; i < geometry.num_rotors; i++) {
+
+		// Get rotor axis
+		Vector3f axis = geometry.rotors[i].axis;
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		const Vector3f &position = geometry.rotors[i].position;
+
+		// Get coefficients
+		float ct = geometry.rotors[i].thrust_coef;
+		float km = geometry.rotors[i].moment_ratio;
+
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+
+		// Fill corresponding items in effectiveness matrix
+		for (int j = 0; j < 3; j++) {
+			effectiveness(j, i) = moment(j);
+			effectiveness(j + 3, i) = thrust(j);
+		}
+	}
+
 	return effectiveness;
 }
 

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -89,6 +89,16 @@ public:
 		return true;
 	}
 
+	void setCutoffFreq(float cutoff_freq)
+	{
+		if (cutoff_freq > FLT_EPSILON) {
+			_time_constant = 1.f / (M_TWOPI_F * cutoff_freq);
+
+		} else {
+			_time_constant = 0.f;
+		}
+	}
+
 	/**
 	 * Set filter parameter alpha directly without time abstraction
 	 *

src/lib/systemlib/system_params.c

@@ -189,11 +189,10 @@ PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10);
 PARAM_DEFINE_INT32(SYS_HAS_GPS, 1);
 
 /**
- * Control if the vehicle has a magnetometer
+ * Control if and how many magnetometers are expected
  *
- * Set this to 0 if the board has no magnetometer.
- * If set to 0, the preflight checks will not check for the presence of a
- * magnetometer, otherwise N sensors are required.
+ * 0: System has no magnetometer, preflight checks should pass without one.
+ * 1-N: Require the presence of N magnetometer sensors for check to pass.
  *
  * @reboot_required true
  * @group System

src/modules/commander/Commander.cpp

@@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm()
 				_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
 
 			} else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) {
-				_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s);
+				_auto_disarm_landed.set_hysteresis_time_from(false,
+						(_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s);
 				_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
 			}
 

src/modules/commander/Commander.hpp

@@ -330,7 +330,6 @@ private:
 	param_t _param_rc_map_fltmode{PARAM_INVALID};
 
 	DEFINE_PARAMETERS(
-
 		(ParamFloat<px4::params::COM_DISARM_LAND>)  _param_com_disarm_land,
 		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_prflt,
 		(ParamBool<px4::params::COM_DISARM_MAN>)    _param_com_disarm_man,
@@ -347,6 +346,7 @@ private:
 		(ParamFloat<px4::params::COM_OBC_LOSS_T>)   _param_com_obc_loss_t,
 		(ParamInt<px4::params::COM_PREARM_MODE>)    _param_com_prearm_mode,
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_com_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_com_takeoff_act,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -73,6 +73,14 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co
 	addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index);
 }
 
+void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component,
+				uint32_t event_id, const events::LogLevels &log_levels, const char *message)
+{
+	armingCheckFailure(required_modes.fail_modes, component, log_levels.external);
+	addEvent(event_id, log_levels, message,
+		 (uint32_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index);
+}
+
 Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels,
 		uint32_t modes, unsigned args_size)
 {

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -74,6 +74,12 @@ static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatc
 static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t),
 	      "type too small, use next larger type");
 
+// Type to pass two mode groups in one struct to have the same number of function arguments to facilitate events parsing
+struct NavModesMessageFail {
+	NavModes message_modes; ///< modes in which there's user messageing but arming is allowed
+	NavModes fail_modes; ///< modes in which checks fail which must be a subset of message_modes
+};
+
 static inline NavModes operator|(NavModes a, NavModes b)
 {
 	return static_cast<NavModes>(static_cast<uint32_t>(a) | static_cast<uint32_t>(b));
@@ -251,6 +257,14 @@ public:
 	void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
 				const events::LogLevels &log_levels, const char *message);
 
+	/**
+	 * Overloaded variant of armingCheckFailure() which allows to separately specify modes in which a message should be emitted and a subset in which arming is blocked
+	 * @param required_modes .message_modes modes in which to put out the event and hence user message.
+	 *                       .failing_modes modes in which to to fail arming. Has to be a subset of message_modes to never disallow arming without a reason.
+	 */
+	void armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component,
+				uint32_t event_id, const events::LogLevels &log_levels, const char *message);
+
 	void clearArmingBits(NavModes modes);
 
 	/**

src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp

@@ -206,19 +206,62 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 					  || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None;
 		events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold
 					     ? events::Log::Critical : events::Log::Warning;
-		/* EVENT
-		 * @description
-		 * The battery state of charge of the worst battery is below the threshold.
-		 *
-		 * <profile name="dev">
-		 * This check can be configured via <param>BAT_LOW_THR</param>, <param>BAT_CRIT_THR</param>, <param>BAT_EMERGEN_THR</param> and <param>COM_ARM_BAT_MIN</param> parameters.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level,
-					    "Low battery");
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t");
+		switch (reporter.failsafeFlags().battery_warning) {
+		default:
+		case battery_status_s::BATTERY_WARNING_LOW:
+			/* EVENT
+			* @description
+			* The lowest battery state of charge is below the low threshold.
+			*
+			* <profile name="dev">
+			* Can be configured with <param>BAT_LOW_THR</param>.
+			* </profile>
+			*/
+			reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"),
+						    log_level, "Low battery");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery\t");
+			}
+
+			break;
+
+		case battery_status_s::BATTERY_WARNING_CRITICAL:
+			/* EVENT
+			* @description
+			* The lowest battery state of charge is below the critical threshold.
+			*
+			* <profile name="dev">
+			* Can be configured with <param>BAT_CRIT_THR</param> and from when to disalow arming with <param>COM_ARM_BAT_MIN</param>.
+			* </profile>
+			*/
+			reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_critical"),
+						    log_level, "Critical battery");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_emergency(reporter.mavlink_log_pub(), "Critical battery\t");
+			}
+
+			break;
+
+		case battery_status_s::BATTERY_WARNING_EMERGENCY:
+			/* EVENT
+			* @description
+			* The lowest battery state of charge is below the emergency threshold.
+			*
+			* <profile name="dev">
+			* Can be configured with <param>BAT_EMERGEN_THR</param>.
+			* </profile>
+			*/
+			reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_emergency"),
+						    log_level, "Emergency battery level");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_emergency(reporter.mavlink_log_pub(), "Emergency battery level\t");
+			}
+
+			break;
 		}
 
 	}

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -273,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 		}
 
 		if (!context.isArmed() && ekf_gps_check_fail) {
-			NavModes required_groups_gps;
+			NavModesMessageFail required_modes;
 			events::Log log_level;
 
 			switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
@@ -281,17 +281,21 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 
 			/* FALLTHROUGH */
 			case GnssArmingCheck::DenyArming:
-				required_groups_gps = required_groups;
+				required_modes.message_modes = required_modes.fail_modes = NavModes::All;
 				log_level = events::Log::Error;
 				break;
 
 			case GnssArmingCheck::WarningOnly:
-				required_groups_gps = NavModes::None; // optional
+				required_modes.message_modes = (NavModes)(reporter.failsafeFlags().mode_req_local_position
+							       | reporter.failsafeFlags().mode_req_local_position_relaxed
+							       | reporter.failsafeFlags().mode_req_global_position);
+				// Only warn and don't block arming because there could still be a valid position estimate from another source e.g. optical flow, VIO
+				required_modes.fail_modes = NavModes::None;
 				log_level = events::Log::Warning;
 				break;
 
 			case GnssArmingCheck::Disabled:
-				required_groups_gps = NavModes::None;
+				required_modes.message_modes = required_modes.fail_modes = NavModes::None;
 				log_level = events::Log::Disabled;
 				break;
 			}
@@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_fix_too_low"),
 							    log_level, "GPS fix too low");
 
@@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_num_sats_too_low"),
 							    log_level, "Not enough GPS Satellites");
 
@@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_pdop_too_high"),
 							    log_level, "GPS PDOP too high");
 
@@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_hor_pos_err_too_high"),
 							    log_level, "GPS Horizontal Position Error too high");
 
@@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_vert_pos_err_too_high"),
 							    log_level, "GPS Vertical Position Error too high");
 
@@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_speed_acc_too_low"),
 							    log_level, "GPS Speed Accuracy too low");
 
@@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
 							    log_level, "GPS Horizontal Position Drift too high");
 
@@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
 							    log_level, "GPS Vertical Position Drift too high");
 
@@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
 							    log_level, "GPS Horizontal Speed Drift too high");
 
@@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
 							    log_level, "GPS Vertical Speed Drift too high");
 
@@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 				/* EVENT
 				 * @description
 				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
 							    events::ID("check_estimator_gps_spoofed"),
 							    log_level, "GPS signal spoofed");
 
@@ -437,7 +441,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 					message = "Preflight%s: Estimator not using GPS";
 					/* EVENT
 					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+					reporter.armingCheckFailure(required_modes, health_component_t::gps,
 								    events::ID("check_estimator_gps_not_fusing"),
 								    log_level, "Estimator not using GPS");
 
@@ -446,7 +450,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 					message = "Preflight%s: Poor GPS Quality";
 					/* EVENT
 					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+					reporter.armingCheckFailure(required_modes, health_component_t::gps,
 								    events::ID("check_estimator_gps_generic"),
 								    log_level, "Poor GPS Quality");
 				}

src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp

@@ -38,6 +38,7 @@
 #include <uORB/topics/arming_check_reply.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
+#include <px4_platform_common/module_params.h>
 
 static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t)
 	      health_component_t::avoidance, "enum definition missmatch");
@@ -66,7 +67,7 @@ public:
 	void update();
 
 	bool isUnresponsive(int registration_id);
-
+	bool allowUpdateWhileArmed() const { return _param_com_mode_arm_chk.get(); }
 private:
 	static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms;
 	static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms;
@@ -109,4 +110,7 @@ private:
 	uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};
 
 	uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::COM_MODE_ARM_CHK>) _param_com_mode_arm_chk
+				       );
 };

src/modules/commander/ModeManagement.cpp

@@ -370,11 +370,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa
 	_failsafe_action_active = failsafe_action_active;
 	_external_checks.update();
 
-	bool allow_update_while_armed = false;
-#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
-	// For simulation, allow registering modes while armed for developer convenience
-	allow_update_while_armed = true;
-#endif
+	bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed();
 
 	if (armed && !allow_update_while_armed) {
 		// Reject registration requests
@@ -408,7 +404,8 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa
 			}
 		}
 
-		// As we're disarmed we can use the user intended mode, as no failsafe will be active
+		// As we're disarmed we can use the user intended mode, as no failsafe will be active.
+		// Note that this might not be true if COM_MODE_ARM_CHK is set
 		checkNewRegistrations(update_request);
 		checkUnregistrations(user_intended_nav_state, update_request);
 	}

src/modules/commander/commander_params.c

@@ -1018,3 +1018,14 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
+
+/**
+ * Allow external mode registration while armed.
+ *
+ * By default disabled for safety reasons
+ *
+ * @group Commander
+ * @boolean
+ *
+ */
+PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0);

src/modules/control_allocator/CMakeLists.txt

@@ -32,8 +32,7 @@
 ############################################################################
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-add_subdirectory(ActuatorEffectiveness)
-add_subdirectory(ControlAllocation)
+add_subdirectory(VehicleActuatorEffectiveness)
 
 px4_add_module(
 	MODULE modules__control_allocator
@@ -50,6 +49,7 @@ px4_add_module(
 	DEPENDS
 		mathlib
 		ActuatorEffectiveness
+		VehicleActuatorEffectiveness
 		ControlAllocation
 		px4_work_queue
 		SlewRate

src/modules/control_allocator/ControlAllocator.cpp

@@ -314,7 +314,7 @@ ControlAllocator::Run()
 #endif
 
 	// Check if parameters have changed
-	if (_parameter_update_sub.updated() && !_armed) {
+	if (_parameter_update_sub.updated()) {
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);

src/modules/control_allocator/Kconfig

@@ -10,3 +10,10 @@ menuconfig USER_CONTROL_ALLOCATOR
 	depends on BOARD_PROTECTED && MODULES_CONTROL_ALLOCATOR
 	---help---
 		Put control_allocator in userspace memory
+
+menuconfig CONTROL_ALLOCATOR_RPM_CONTROL
+	bool "Include RPM control for Helicopter rotor"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Add support for controlling the helicopter main rotor rpm

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 #include <px4_platform_common/module_params.h>
 #include <lib/slew_rate/SlewRate.hpp>

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -32,7 +32,6 @@
  ****************************************************************************/
 
 #include "ActuatorEffectivenessFixedWing.hpp"
-#include <ControlAllocation/ControlAllocation.hpp>
 
 using namespace matrix;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -134,9 +134,16 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 {
 	_saturation_flags = {};
 
+	const float spoolup_progress = throttleSpoolupProgress();
+	float rpm_control_output = 0;
+#if CONTROL_ALLOCATOR_RPM_CONTROL
+	_rpm_control.setSpoolupProgress(spoolup_progress);
+	rpm_control_output = _rpm_control.getActuatorCorrection();
+#endif // CONTROL_ALLOCATOR_RPM_CONTROL
+
 	// throttle/collective pitch curve
-	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
-			       _geometry.throttle_curve) * throttleSpoolupProgress();
+	const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.throttle_curve)
+				+ rpm_control_output) * spoolup_progress;
 	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
 
 	// actuator mapping

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 #include <px4_platform_common/module_params.h>
 
@@ -41,6 +41,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/manual_control_switches.h>
 
+#include "RpmControl.hpp"
+
 class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
 {
 public:
@@ -131,4 +133,8 @@ private:
 	bool _main_motor_engaged{true};
 
 	const ActuatorType _tail_actuator_type;
+
+#if CONTROL_ALLOCATOR_RPM_CONTROL
+	RpmControl _rpm_control {this};
+#endif // CONTROL_ALLOCATOR_RPM_CONTROL
 };

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 #include <px4_platform_common/module_params.h>
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessTilts.hpp"
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 
 class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -41,7 +41,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 #include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -32,7 +32,6 @@
  ****************************************************************************/
 
 #include "ActuatorEffectivenessRoverAckermann.hpp"
-#include <ControlAllocation/ControlAllocation.hpp>
 
 using namespace matrix;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness
 {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -32,7 +32,6 @@
  ****************************************************************************/
 
 #include "ActuatorEffectivenessStandardVTOL.hpp"
-#include <ControlAllocation/ControlAllocation.hpp>
 
 using namespace matrix;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -41,7 +41,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -41,7 +41,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 #include "ActuatorEffectivenessTilts.hpp"

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 
 #include <px4_platform_common/module_params.h>

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 #include "ActuatorEffectivenessRotors.hpp"
 
 class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness

src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt

@@ -0,0 +1,43 @@
+px4_add_library(VehicleActuatorEffectiveness
+	ActuatorEffectivenessUUV.cpp
+	ActuatorEffectivenessUUV.hpp
+	ActuatorEffectivenessControlSurfaces.cpp
+	ActuatorEffectivenessControlSurfaces.hpp
+	ActuatorEffectivenessCustom.cpp
+	ActuatorEffectivenessCustom.hpp
+	ActuatorEffectivenessFixedWing.cpp
+	ActuatorEffectivenessFixedWing.hpp
+	ActuatorEffectivenessHelicopter.cpp
+	ActuatorEffectivenessHelicopter.hpp
+	ActuatorEffectivenessHelicopterCoaxial.cpp
+	ActuatorEffectivenessHelicopterCoaxial.hpp
+	ActuatorEffectivenessMCTilt.cpp
+	ActuatorEffectivenessMCTilt.hpp
+	ActuatorEffectivenessMultirotor.cpp
+	ActuatorEffectivenessMultirotor.hpp
+	ActuatorEffectivenessTilts.cpp
+	ActuatorEffectivenessTilts.hpp
+	ActuatorEffectivenessRotors.cpp
+	ActuatorEffectivenessRotors.hpp
+	ActuatorEffectivenessStandardVTOL.cpp
+	ActuatorEffectivenessStandardVTOL.hpp
+	ActuatorEffectivenessTiltrotorVTOL.cpp
+	ActuatorEffectivenessTiltrotorVTOL.hpp
+	ActuatorEffectivenessTailsitterVTOL.cpp
+	ActuatorEffectivenessTailsitterVTOL.hpp
+	ActuatorEffectivenessRoverAckermann.hpp
+	ActuatorEffectivenessRoverAckermann.cpp
+	RpmControl.hpp
+	RpmControl.cpp
+)
+
+target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(VehicleActuatorEffectiveness
+	PRIVATE
+		mathlib
+		ActuatorEffectiveness
+)
+
+px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness)
+px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS VehicleActuatorEffectiveness)

src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "RpmControl.hpp"
+
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_pid.setOutputLimit(PID_OUTPUT_LIMIT);
+	_pid.setIntegralLimit(PID_OUTPUT_LIMIT);
+};
+
+void RpmControl::setSpoolupProgress(float spoolup_progress)
+{
+	_spoolup_progress = spoolup_progress;
+	_pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get());
+
+	if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) {
+		_pid.resetIntegral();
+	}
+}
+
+float RpmControl::getActuatorCorrection()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	// RPM measurement update
+	if (_rpm_sub.updated()) {
+		rpm_s rpm{};
+
+		if (_rpm_sub.copy(&rpm)) {
+			const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f);
+			_timestamp_last_measurement = rpm.timestamp;
+
+			const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f);
+			_pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f);
+			_actuator_correction = _pid.update(rpm.rpm_estimate, dt, true);
+
+			_rpm_invalid = rpm.rpm_estimate < 1.f;
+		}
+	}
+
+	// Timeout
+	const bool timeout = now > _timestamp_last_measurement + 1_s;
+
+	if (_rpm_invalid || timeout) {
+		_pid.resetIntegral();
+		_actuator_correction = 0.f;
+	}
+
+	return _actuator_correction;
+}

src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 RpmControl.hpp
+ *
+ * Control rpm of a helicopter rotor.
+ * Input: PWM input pulse period from an rpm sensor
+ * Output: Duty cycle command for the ESC
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/pid/PID.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rpm.h>
+
+class RpmControl : public ModuleParams
+{
+public:
+	RpmControl(ModuleParams *parent);
+	~RpmControl() = default;
+
+	void setSpoolupProgress(float spoolup_progress);
+	float getActuatorCorrection();
+
+private:
+	static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1]
+	static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1]
+
+	uORB::Subscription _rpm_sub{ORB_ID(rpm)};
+	bool _rpm_invalid{true};
+	PID _pid;
+	float _spoolup_progress{0.f}; // [0,1]
+	hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout
+	float _actuator_correction{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CA_HELI_RPM_SP>) _param_ca_heli_rpm_sp,
+		(ParamFloat<px4::params::CA_HELI_RPM_P>) _param_ca_heli_rpm_p,
+		(ParamFloat<px4::params::CA_HELI_RPM_I>) _param_ca_heli_rpm_i
+	)
+};

src/modules/control_allocator/module.yaml

@@ -71,15 +71,14 @@ parameters:
             description:
                 short: Motor ${i} slew rate limit
                 long: |
-                  Minimum time allowed for the motor input signal to pass through
-                  the full output range. A value x means that the motor signal
-                  can only go from 0 to 1 in minimum x seconds (in case of
-                  reversible motors, the range is -1 to 1).
+                  Forces the motor output signal to take at least the configured time (in seconds)
+                  to traverse its full range (normally [0, 1], or if reversible [-1, 1]).
 
                   Zero means that slew rate limiting is disabled.
             type: float
             decimal: 2
             increment: 0.01
+            unit: s
             num_instances: *max_num_mc_motors
             min: 0
             max: 10
@@ -90,14 +89,14 @@ parameters:
             description:
                 short: Servo ${i} slew rate limit
                 long: |
-                  Minimum time allowed for the servo input signal to pass through
-                  the full output range. A value x means that the servo signal
-                  can only go from -1 to 1 in minimum x seconds.
+                  Forces the servo output signal to take at least the configured time (in seconds)
+                  to traverse its full range [-100%, 100%].
 
                   Zero means that slew rate limiting is disabled.
             type: float
             decimal: 2
             increment: 0.05
+            unit: s
             num_instances: *max_num_servos
             min: 0
             max: 10
@@ -528,6 +527,41 @@ parameters:
               which is mostly the case when the main rotor turns counter-clockwise.
           type: boolean
           default: 0
+        CA_HELI_RPM_SP:
+          description:
+            short: Setpoint for main rotor rpm
+            long: |
+              Requires rpm feedback for the controller.
+          type: float
+          decimal: 0
+          increment: 1
+          min: 100
+          max: 10000
+          default: 1500
+        CA_HELI_RPM_P:
+          description:
+            short: Proportional gain for rpm control
+            long: |
+              Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it.
+
+              motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: 0
+          max: 10
+          default: 0.0
+        CA_HELI_RPM_I:
+          description:
+            short: Integral gain for rpm control
+            long: |
+              Same definition as the proportional gain but for integral.
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: 0
+          max: 10
+          default: 0.0
 
         # Others
         CA_FAILURE_MODE:

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -65,6 +65,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 			if ((_baro_counter == 0) || baro_sample.reset) {
 				_baro_lpf.reset(measurement);
 				_baro_counter = 1;
+				_control_status.flags.baro_fault = false;
 
 			} else {
 				_baro_lpf.update(measurement);
@@ -113,7 +114,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
 				&& measurement_valid
 				&& (_baro_counter > _obs_buffer_length)
-				&& !_baro_hgt_faulty;
+				&& !_control_status.flags.baro_fault;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
@@ -148,7 +149,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 
 					if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) {
 						// Some other height source is still working
-						_baro_hgt_faulty = true;
+						_control_status.flags.baro_fault = true;
 					}
 				}
 

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -102,11 +102,13 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 
 		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
 		const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
-		Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
-		pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
+		Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
 
 		// Apply a low pass filter
-		_gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
+		_gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
+
+		// Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals
+		_gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit);
 
 		// hdrift: calculate the horizontal drift speed and fail if too high
 		_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
 		return false;
 	}
 
-	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
+	const float yaw_var = _yawEstimator.getYawVar();
+
+	return (yaw_var > 0.f)
+	       && (yaw_var < sq(_params.EKFGSF_yaw_err_max))
+	       && PX4_ISFINITE(yaw_var);
 }
 
 bool Ekf::isYawFailure() const

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

@@ -43,14 +43,14 @@
 
 #include "ekf.h"
 #include <ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h>
-#include <ekf_derivation/generated/compute_sideslip_h_and_k.h>
+#include <ekf_derivation/generated/compute_sideslip_h.h>
 
 #include <mathlib/mathlib.h>
 
 void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 {
 	_control_status.flags.fuse_beta = _params.beta_fusion_enabled
-					  && _control_status.flags.fixed_wing
+					  && (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd)
 					  && _control_status.flags.in_air
 					  && !_control_status.flags.fake_pos;
 
@@ -127,10 +127,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	_fault_status.flags.bad_sideslip = false;
 
 	const float epsilon = 1e-3f;
-	VectorState H; // Observation jacobian
-	VectorState K; // Kalman gain vector
 
-	sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K);
+	const VectorState H = sym::ComputeSideslipH(_state.vector(), epsilon);
+	VectorState K = P * H / sideslip.innovation_variance;
 
 	if (update_wind_only) {
 		const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
@@ -143,7 +142,5 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	sideslip.fused = true;
 	sideslip.time_last_fuse = _time_delayed_us;
 
-	_fault_status.flags.bad_sideslip = false;
-
 	return true;
 }

src/modules/ekf2/EKF/common.h

@@ -620,6 +620,7 @@ uint64_t mag_heading_consistent  :
 		uint64_t opt_flow_terrain        : 1; ///< 40 - true if we are fusing flow data for terrain
 		uint64_t valid_fake_pos          : 1; ///< 41 - true if a valid constant position is being fused
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
+		uint64_t baro_fault	      : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
 
 	} flags;
 	uint64_t value;

src/modules/ekf2/EKF/covariance.cpp

@@ -280,7 +280,17 @@ void Ekf::constrainStateVariances()
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 {
 	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
-		P(i, i) = math::constrain(P(i, i), min, max);
+		if (P(i, i) < min) {
+			P(i, i) = min;
+
+		} else if (P(i, i) > max) {
+			// Constrain the variance growth by fusing zero innovation as clipping the variance
+			// would artifically increase the correlation between states and destabilize the filter.
+			const float innov = 0.f;
+			const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R)
+			const float innov_var = P(i, i) + R;
+			fuseDirectStateMeasurement(innov, innov_var, R, i);
+		}
 	}
 }
 
@@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
 	float limited_max = math::constrain(state_var_max, min, max);
 	float limited_min = math::constrain(limited_max / max_ratio, min, max);
 
-	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
-		P(i, i) = math::constrain(P(i, i), limited_min, limited_max);
-	}
+	constrainStateVar(state, limited_min, limited_max);
 }
 
 void Ekf::resetQuatCov(const float yaw_noise)

src/modules/ekf2/EKF/ekf.h

@@ -208,8 +208,9 @@ public:
 	//  vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
 	//  vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
 	//  hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
-	// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
-	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
+	//  hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed.
+	//  hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed.
+	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const;
 
 	void resetGyroBias();
 	void resetGyroBiasCov();
@@ -601,7 +602,6 @@ private:
 
 	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
 
-	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -327,13 +327,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
 }
 
-void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
+void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z,
+			      float *hagl_max_xy) const
 {
 	// Do not require limiting by default
 	*vxy_max = NAN;
 	*vz_max = NAN;
 	*hagl_min = NAN;
-	*hagl_max = NAN;
+	*hagl_max_z = NAN;
+	*hagl_max_xy = NAN;
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Calculate range finder limits
@@ -347,7 +349,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 
 	if (relying_on_rangefinder) {
 		*hagl_min = rangefinder_hagl_min;
-		*hagl_max = rangefinder_hagl_max;
+		*hagl_max_z = rangefinder_hagl_max;
 	}
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -370,11 +372,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);
 
 		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
-		const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
+		float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
+		flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f);
 
 		*vxy_max = flow_vxy_max;
 		*hagl_min = flow_hagl_min;
-		*hagl_max = flow_hagl_max;
+		*hagl_max_xy = flow_hagl_max;
 	}
 
 # endif // CONFIG_EKF2_OPTICAL_FLOW

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

@@ -354,10 +354,8 @@ def compute_sideslip_innov_and_innov_var(
 
     return (innov, innov_var)
 
-def compute_sideslip_h_and_k(
+def compute_sideslip_h(
         state: VState,
-        P: MTangent,
-        innov_var: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (VTangent, VTangent):
 
@@ -366,9 +364,7 @@ def compute_sideslip_h_and_k(
 
     H = jacobian_chain_rule(sideslip_pred, state)
 
-    K = P * H.T / sf.Max(innov_var, epsilon)
-
-    return (H.T, K)
+    return (H.T)
 
 def predict_vel_body(
         state: VState
@@ -739,7 +735,7 @@ if not args.disable_wind:
     generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"])
     generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"])
-    generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
+    generate_px4_function(compute_sideslip_h, output_names=None)
     generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
     generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"])

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

@@ -0,0 +1,96 @@
+// -----------------------------------------------------------------------------
+// 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_sideslip_h
+ *
+ * Args:
+ *     state: Matrix25_1
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     res: Matrix24_1
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 24, 1> ComputeSideslipH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                              const Scalar epsilon) {
+  // Total ops: 131
+
+  // Input arrays
+
+  // Intermediate terms (37)
+  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 = 2 * state(6, 0);
+  const Scalar _tmp3 = _tmp2 * state(3, 0);
+  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = 2 * state(0, 0);
+  const Scalar _tmp7 = _tmp6 * state(3, 0);
+  const Scalar _tmp8 = 2 * state(2, 0);
+  const Scalar _tmp9 = _tmp8 * state(1, 0);
+  const Scalar _tmp10 = _tmp7 + _tmp9;
+  const Scalar _tmp11 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp8 * state(0, 0);
+  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp11 + _tmp12 * state(6, 0);
+  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
+  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
+  const Scalar _tmp16 = _tmp2 * state(0, 0);
+  const Scalar _tmp17 = std::pow(_tmp14, Scalar(2));
+  const Scalar _tmp18 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp19 = -_tmp7 + _tmp9;
+  const Scalar _tmp20 = _tmp6 * state(1, 0) + _tmp8 * state(3, 0);
+  const Scalar _tmp21 = _tmp0 * _tmp19 + _tmp11 * _tmp18 + _tmp20 * state(6, 0);
+  const Scalar _tmp22 = _tmp21 / _tmp17;
+  const Scalar _tmp23 = _tmp17 / (_tmp17 + std::pow(_tmp21, Scalar(2)));
+  const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * _tmp23;
+  const Scalar _tmp25 = _tmp24 * (_tmp15 * (_tmp0 * _tmp1 + _tmp3) -
+                                  _tmp22 * (-4 * _tmp0 * state(2, 0) + _tmp1 * _tmp11 - _tmp16));
+  const Scalar _tmp26 = 2 * state(3, 0);
+  const Scalar _tmp27 = _tmp2 * state(1, 0);
+  const Scalar _tmp28 = _tmp2 * state(2, 0);
+  const Scalar _tmp29 =
+      _tmp24 * (_tmp15 * (-_tmp0 * _tmp26 + _tmp27) - _tmp22 * (_tmp11 * _tmp26 - _tmp28));
+  const Scalar _tmp30 = _tmp24 * (_tmp15 * (_tmp0 * _tmp8 - 4 * _tmp11 * state(1, 0) + _tmp16) -
+                                  _tmp22 * (_tmp11 * _tmp8 + _tmp3));
+  const Scalar _tmp31 = 4 * state(3, 0);
+  const Scalar _tmp32 = _tmp24 * (_tmp15 * (-_tmp0 * _tmp6 - _tmp11 * _tmp31 + _tmp28) -
+                                  _tmp22 * (-_tmp0 * _tmp31 + _tmp11 * _tmp6 + _tmp27));
+  const Scalar _tmp33 = _tmp22 * _tmp5;
+  const Scalar _tmp34 = _tmp15 * _tmp19;
+  const Scalar _tmp35 = _tmp15 * _tmp18;
+  const Scalar _tmp36 = _tmp10 * _tmp22;
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 24, 1> _res;
+
+  _res.setZero();
+
+  _res(0, 0) =
+      -_tmp25 * state(3, 0) - _tmp29 * state(1, 0) + _tmp30 * state(0, 0) + _tmp32 * state(2, 0);
+  _res(1, 0) =
+      _tmp25 * state(0, 0) - _tmp29 * state(2, 0) + _tmp30 * state(3, 0) - _tmp32 * state(1, 0);
+  _res(2, 0) =
+      _tmp25 * state(1, 0) - _tmp29 * state(3, 0) - _tmp30 * state(2, 0) + _tmp32 * state(0, 0);
+  _res(3, 0) = _tmp23 * (-_tmp33 + _tmp34);
+  _res(4, 0) = _tmp23 * (_tmp35 - _tmp36);
+  _res(5, 0) = _tmp23 * (-_tmp12 * _tmp22 + _tmp15 * _tmp20);
+  _res(21, 0) = _tmp23 * (_tmp33 - _tmp34);
+  _res(22, 0) = _tmp23 * (-_tmp35 + _tmp36);
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

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

@@ -1,188 +0,0 @@
-// -----------------------------------------------------------------------------
-// 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_sideslip_h_and_k
- *
- * Args:
- *     state: Matrix25_1
- *     P: Matrix24_24
- *     innov_var: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     H: Matrix24_1
- *     K: Matrix24_1
- */
-template <typename Scalar>
-void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
-                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 518
-
-  // Input arrays
-
-  // Intermediate terms (50)
-  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp1 = 2 * _tmp0;
-  const Scalar _tmp2 = 2 * state(3, 0);
-  const Scalar _tmp3 = _tmp2 * state(6, 0);
-  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp6 = _tmp2 * state(0, 0);
-  const Scalar _tmp7 = 2 * state(1, 0);
-  const Scalar _tmp8 = _tmp7 * state(2, 0);
-  const Scalar _tmp9 = _tmp6 + _tmp8;
-  const Scalar _tmp10 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp11 = 2 * state(2, 0);
-  const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0);
-  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0);
-  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
-  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
-  const Scalar _tmp16 = 2 * _tmp10;
-  const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0);
-  const Scalar _tmp18 = std::pow(_tmp14, Scalar(2));
-  const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp20 = -_tmp6 + _tmp8;
-  const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0);
-  const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0);
-  const Scalar _tmp23 = _tmp22 / _tmp18;
-  const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) -
-                        _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17);
-  const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2)));
-  const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25;
-  const Scalar _tmp27 = _tmp26 * state(3, 0);
-  const Scalar _tmp28 = _tmp7 * state(6, 0);
-  const Scalar _tmp29 = _tmp11 * state(6, 0);
-  const Scalar _tmp30 =
-      _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29);
-  const Scalar _tmp31 = _tmp26 * state(1, 0);
-  const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) -
-                        _tmp23 * (_tmp16 * state(2, 0) + _tmp3);
-  const Scalar _tmp33 = _tmp26 * state(0, 0);
-  const Scalar _tmp34 = 4 * state(3, 0);
-  const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) -
-                        _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28);
-  const Scalar _tmp36 = _tmp26 * state(2, 0);
-  const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36;
-  const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35;
-  const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35;
-  const Scalar _tmp40 = _tmp23 * _tmp5;
-  const Scalar _tmp41 = _tmp15 * _tmp20;
-  const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41);
-  const Scalar _tmp43 = _tmp15 * _tmp19;
-  const Scalar _tmp44 = _tmp23 * _tmp9;
-  const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44);
-  const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21);
-  const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41);
-  const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44);
-  const Scalar _tmp49 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
-
-  // Output terms (2)
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
-
-    _h.setZero();
-
-    _h(0, 0) = _tmp37;
-    _h(1, 0) = _tmp38;
-    _h(2, 0) = _tmp39;
-    _h(3, 0) = _tmp42;
-    _h(4, 0) = _tmp45;
-    _h(5, 0) = _tmp46;
-    _h(21, 0) = _tmp47;
-    _h(22, 0) = _tmp48;
-  }
-
-  if (K != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _k = (*K);
-
-    _k(0, 0) =
-        _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 +
-                  P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46);
-    _k(1, 0) =
-        _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 +
-                  P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46);
-    _k(2, 0) =
-        _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 +
-                  P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46);
-    _k(3, 0) =
-        _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 +
-                  P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46);
-    _k(4, 0) =
-        _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 +
-                  P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46);
-    _k(5, 0) =
-        _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 +
-                  P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46);
-    _k(6, 0) =
-        _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 +
-                  P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46);
-    _k(7, 0) =
-        _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 +
-                  P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46);
-    _k(8, 0) =
-        _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 +
-                  P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46);
-    _k(9, 0) =
-        _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 +
-                  P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46);
-    _k(10, 0) =
-        _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 +
-                  P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46);
-    _k(11, 0) =
-        _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 +
-                  P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46);
-    _k(12, 0) =
-        _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 +
-                  P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46);
-    _k(13, 0) =
-        _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 +
-                  P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46);
-    _k(14, 0) =
-        _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 +
-                  P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46);
-    _k(15, 0) =
-        _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 +
-                  P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46);
-    _k(16, 0) =
-        _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 +
-                  P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46);
-    _k(17, 0) =
-        _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 +
-                  P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46);
-    _k(18, 0) =
-        _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 +
-                  P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46);
-    _k(19, 0) =
-        _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 +
-                  P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46);
-    _k(20, 0) =
-        _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 +
-                  P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46);
-    _k(21, 0) =
-        _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 +
-                  P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46);
-    _k(22, 0) =
-        _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 +
-                  P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46);
-    _k(23, 0) =
-        _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 +
-                  P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46);
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp

@@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
 			const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
 			_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta);
 		}
+
+		if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) {
+			reset();
+		}
 	}
 }
 

src/modules/ekf2/EKF2.cpp

@@ -745,6 +745,7 @@ void EKF2::Run()
 		ekf2_timestamps_s ekf2_timestamps {
 			.timestamp = now,
 			.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
+			.airspeed_validated_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
@@ -1628,7 +1629,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 			      || _ekf.control_status_flags().wind_dead_reckoning;
 
 	// get control limit information
-	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
+	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy);
 
 	// convert NaN to INFINITY
 	if (!PX4_ISFINITE(lpos.vxy_max)) {
@@ -1643,8 +1644,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 		lpos.hagl_min = INFINITY;
 	}
 
-	if (!PX4_ISFINITE(lpos.hagl_max)) {
-		lpos.hagl_max = INFINITY;
+	if (!PX4_ISFINITE(lpos.hagl_max_z)) {
+		lpos.hagl_max_z = INFINITY;
+	}
+
+	if (!PX4_ISFINITE(lpos.hagl_max_xy)) {
+		lpos.hagl_max_xy = INFINITY;
 	}
 
 	// publish vehicle local position data
@@ -1928,6 +1933,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_opt_flow_terrain    = _ekf.control_status_flags().opt_flow_terrain;
 		status_flags.cs_valid_fake_pos      = _ekf.control_status_flags().valid_fake_pos;
 		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
+		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
@@ -2076,6 +2082,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			}
 
 			_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
+
+			ekf2_timestamps.airspeed_validated_timestamp_rel = (int16_t)((int64_t)airspeed_validated.timestamp / 100 -
+					(int64_t)ekf2_timestamps.timestamp / 100);
 		}
 
 	} else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {

src/modules/ekf2/EKF2.hpp

@@ -573,12 +573,9 @@ private:
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)
-		(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
-		_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
-		(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
-
-		(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
-		_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
+		(ParamExtFloat<px4::params::EKF2_BETA_GATE>) _param_ekf2_beta_gate,
+		(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise,
+		(ParamExtInt<px4::params::EKF2_FUSE_BETA>) _param_ekf2_fuse_beta,
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)

src/modules/ekf2/Kconfig

@@ -34,7 +34,7 @@ depends on MODULES_EKF2
 menuconfig EKF2_AUX_GLOBAL_POSITION
 depends on MODULES_EKF2
 	bool "aux global position fusion support"
-	default n
+	default y
 	---help---
 		EKF2 auxiliary global position fusion support.
 

src/modules/ekf2/params_range_finder.yaml

@@ -158,7 +158,7 @@ parameters:
           If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the
           measurement may gets rejected. 0 - disabled
       type: float
-      default: 1.0
+      default: 3.0
       min: 0.0
       max: 20.0
       unit: m

src/modules/ekf2/params_sideslip.yaml

@@ -6,8 +6,8 @@ parameters:
       description:
         short: Enable synthetic sideslip fusion
         long: 'For reliable wind estimation both sideslip and airspeed fusion (see
-          EKF2_ARSP_THR) should be enabled. Only applies to fixed-wing vehicles (or
-          VTOLs in fixed-wing mode). Note: side slip fusion is currently not supported
+          EKF2_ARSP_THR) should be enabled. Only applies to vehicles in fixed-wing mode
+          or with airspeed fusion active. Note: side slip fusion is currently not supported
           for tailsitters.'
       type: boolean
       default: 0

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -282,12 +282,6 @@ void FlightTaskAuto::_prepareLandSetpoints()
 			sticks_xy.setZero();
 		}
 
-		// If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed
-		if (PX4_ISFINITE(_dist_to_bottom)) {
-			// Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed
-			max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f));
-		}
-
 		_stick_acceleration_xy.setVelocityConstraint(max_speed);
 		_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
 				_velocity_setpoint_feedback.xy(), _deltatime);

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -59,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 
 bool FlightTaskManualAcceleration::update()
 {
+	const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get();
+	setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy);
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
+	float max_hagl_ratio = 0.0f;
+
+	if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) {
+		max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy;
+	}
+
+	// limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps
+	static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl
+	static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl
+
+	if (max_hagl_ratio > factor_threshold) {
+		max_hagl_ratio = math::min(max_hagl_ratio, 1.f);
+		const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
+		_stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel));
+
+	} else {
+		_stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max));
+	}
+
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -52,4 +52,9 @@ protected:
 
 	StickAccelerationXY _stick_acceleration_xy{this};
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
+				       )
 };