feat(control_alloctor): Add actuator group preflight check

The current preflight check works by sending individual actuator
commands over mavlink. This can only be done sequentially, so the
surfaces do not move in a coordinated way, instilling little confidence.

To alleviate, this adds a control surface preflight check which causes
the allocator to command torque in the given direction, so the control
surfaces move as they would in flight.

Triggered by `{MAV,VEHICLE}_CMD_ACTUATOR_GROUP_TEST`. Only runs when
pre-armed (control surfaces, tilts) or armed (thrust).

src/modules/commander/Commander.cpp

@@ -1640,6 +1640,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE:
 	case vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE:
 	case vehicle_command_s::VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE:
+	case vehicle_command_s::VEHICLE_CMD_ACTUATOR_GROUP_TEST:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 

src/modules/control_allocator/ActuatorGroupPreflightCheck.cpp

@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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 "ActuatorGroupPreflightCheck.hpp"
+
+bool ActuatorGroupPreflightCheck::isThrust(uint8_t group)
+{
+	// Thrust groups require different handling for a couple reasons:
+	//  - requires being armed to be useful
+	//  - want longer duration (thrust can have larger slew rate)
+	return group == vehicle_command_s::ACTUATOR_TEST_GROUP_X_THRUST
+	       || group == vehicle_command_s::ACTUATOR_TEST_GROUP_Y_THRUST
+	       || group == vehicle_command_s::ACTUATOR_TEST_GROUP_Z_THRUST;
+}
+
+bool ActuatorGroupPreflightCheck::isKnownGroup(uint8_t group)
+{
+	switch (group) {
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_ROLL_TORQUE:
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_PITCH_TORQUE:
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_YAW_TORQUE:
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT:
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_X_THRUST:
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_Y_THRUST:
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_Z_THRUST:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+const char *ActuatorGroupPreflightCheck::validateCommand(uint8_t group, bool is_tiltrotor, uint8_t &ack_result)
+{
+	// Permanent failures: the airframe does not support the command
+	ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+	if (!isKnownGroup(group)) {
+		return "unknown actuator group";
+	}
+
+	if (group == vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT && !is_tiltrotor) {
+		ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+		return "not a tiltrotor";
+	}
+
+	// Genuine internal failure: we could not read the state we need
+	ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+
+	actuator_armed_s actuator_armed;
+
+	if (!_armed_sub.copy(&actuator_armed)) {
+		return "failed to read armed state";
+	}
+
+	// Transient: arming state mismatch. The user can change it and retry.
+	ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+	if (isThrust(group)) {
+
+		if (!actuator_armed.armed) {
+			return "thrust test requires armed";
+		}
+
+	} else {
+
+		if (actuator_armed.armed) {
+			return "armed";
+		}
+
+		if (!actuator_armed.prearmed) {
+			return "not prearmed";
+		}
+	}
+
+	return nullptr;
+}
+
+void ActuatorGroupPreflightCheck::handleCommand(hrt_abstime now, bool is_tiltrotor)
+{
+	vehicle_command_s vehicle_command;
+
+	if (!_vehicle_command_sub.update(&vehicle_command)) { return; }
+
+	if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_ACTUATOR_GROUP_TEST) { return; }
+
+	const uint8_t group = (uint8_t) lroundf(vehicle_command.param1);
+
+	uint8_t reject_result;  // Is written to by validateCommand
+	const char *reject_reason = validateCommand(group, is_tiltrotor, reject_result);
+
+	if (reject_reason) {
+		PX4_WARN("Actuator group preflight check rejected (%s)", reject_reason);
+		// Ack the rejected command directly without changing _last_command, so any check
+		// currently running can still respond to its own requester.
+		sendAck(vehicle_command, reject_result, now);
+		return;
+	}
+
+	if (_running) {
+		// Cancel the previous check, still targeted at its original requester.
+		sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
+	}
+
+	// Torque and thrust have inputs in [-1, 1]. Tilt expects [0, 1] (0=vertical, 1=horizontal);
+	const float min_input = group == vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT ? 0.f : -1.f;
+
+	_group = group;
+	_requires_armed = isThrust(group);
+	_input = math::constrain(vehicle_command.param2, min_input, 1.f);
+	_started = now;
+	_running = true;
+
+	_last_command = vehicle_command;
+	sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS, now);
+}
+
+void ActuatorGroupPreflightCheck::updateState(hrt_abstime now)
+{
+	if (!_running) { return; }
+
+	actuator_armed_s actuator_armed;
+
+	if (_armed_sub.copy(&actuator_armed)) {
+		// Cancel if any of the conditions we depended on at start are lost
+		// (thrust -> armed, torque/tilt -> pre-armed).
+		const bool armed_lost = actuator_armed.armed != _requires_armed;
+		const bool prearmed_lost = !_requires_armed && !actuator_armed.prearmed;
+
+		if (armed_lost || prearmed_lost) {
+			_running = false;
+			sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
+			return;
+		}
+	}
+
+	const hrt_abstime duration = isThrust(_group) ? PREFLIGHT_CHECK_DURATION_THRUST : PREFLIGHT_CHECK_DURATION_SERVOS;
+
+	if (now - _started >= duration) {
+		_running = false;
+		sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, now);
+	}
+}
+
+void ActuatorGroupPreflightCheck::applyOverrides(matrix::Vector<float, NUM_AXES> (&c)[MAX_NUM_MATRICES],
+		bool is_vtol, ActuatorEffectiveness &effectiveness)
+{
+	bool do_override_tilt = false;
+	int override_index = -1;
+
+	// Map vehicle command constants to indices of the c matrix used in ControlAllocator
+	switch (_group) {
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_ROLL_TORQUE:
+		override_index = 0; break;
+
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_PITCH_TORQUE:
+		override_index = 1; break;
+
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_YAW_TORQUE:
+		override_index = 2; break;
+
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_X_THRUST:
+		override_index = 3; break;
+
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_Y_THRUST:
+		override_index = 4; break;
+
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_Z_THRUST:
+		override_index = 5; break;
+
+	case vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT:
+		do_override_tilt = _running;
+		break;
+	}
+
+	// Always update tilt override, even when not running. We need an
+	// explicit call with do_override_tilt = false to undo the override.
+	effectiveness.overrideCollectiveTilt(do_override_tilt, _input);
+
+	if (_running && override_index >= 0) {
+		// On a VTOL, control surfaces are in instance 1; thrust always lives in instance 0.
+		const int instance = (is_vtol && !isThrust(_group)) ? 1 : 0;
+		c[instance](override_index) = _input;
+	}
+}
+
+void ActuatorGroupPreflightCheck::sendAck(const vehicle_command_s &cmd, uint8_t result, hrt_abstime now)
+{
+	if (cmd.from_external) {
+		vehicle_command_ack_s command_ack{};
+		command_ack.timestamp = now;
+		command_ack.command = cmd.command;
+		command_ack.result = result;
+		command_ack.target_system = cmd.source_system;
+		command_ack.target_component = cmd.source_component;
+		_command_ack_pub.publish(command_ack);
+	}
+}

src/modules/control_allocator/ActuatorGroupPreflightCheck.hpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <ActuatorEffectiveness.hpp>
+#include <ControlAllocation.hpp>
+
+#include <math.h>
+#include <mathlib/math/Limits.hpp>
+#include <drivers/drv_hrt.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <px4_platform_common/log.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+/**
+ * Drives the VEHICLE_CMD_ACTUATOR_GROUP_TEST state machine: holds a fixed
+ * torque, thrust or collective tilt on a single functional group for a short
+ * duration so the operator can verify actuator motion before flight.
+ *
+ * Arming requirements (enforced both at start and continuously while running):
+ *  - torque and collective-tilt groups: prearmed and *not* armed
+ *  - thrust groups: armed
+ *
+ * Losing the required state mid-check results in a CANCELLED ack.
+ */
+class ActuatorGroupPreflightCheck
+{
+public:
+	static constexpr int NUM_AXES = ControlAllocation::NUM_AXES;
+	static constexpr int MAX_NUM_MATRICES = ActuatorEffectiveness::MAX_NUM_MATRICES;
+
+	ActuatorGroupPreflightCheck() = default;
+	~ActuatorGroupPreflightCheck() = default;
+
+	bool isActive() const { return _running; }
+
+	// Poll vehicle_command for a group test request and start it if conditions allow.
+	void handleCommand(hrt_abstime now, bool is_tiltrotor);
+
+	// Finish the check after the group-specific duration, or abort if the required arming state is lost.
+	void updateState(hrt_abstime now);
+
+	// Apply the test torque/thrust setpoint and/or collective tilt override.
+	void applyOverrides(matrix::Vector<float, NUM_AXES> (&c)[MAX_NUM_MATRICES], bool is_vtol,
+			    ActuatorEffectiveness &effectiveness);
+
+private:
+	static constexpr hrt_abstime PREFLIGHT_CHECK_DURATION_SERVOS = 500'000;  // 500 ms
+	// A bit longer since FW forward thrust can have significant slew rate
+	static constexpr hrt_abstime PREFLIGHT_CHECK_DURATION_THRUST = 2'000'000;  // 2 s
+
+	static bool isThrust(uint8_t group);
+	static bool isKnownGroup(uint8_t group);
+
+	// Returns
+	//  - nullptr, if the command is acceptable
+	//  - char* with rejection reason, if not
+	// On rejection, also writes the appropriate VEHICLE_CMD_RESULT_* into ack_result.
+	const char *validateCommand(uint8_t group, bool is_tiltrotor, uint8_t &ack_result);
+
+	void sendAck(const vehicle_command_s &cmd, uint8_t result, hrt_abstime now);
+
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	bool _running{false};
+	bool _requires_armed{false};
+	uint8_t _group{0};
+	float _input{0.0f};
+	vehicle_command_s _last_command{};
+	hrt_abstime _started{0};
+};

src/modules/control_allocator/CMakeLists.txt

@@ -44,6 +44,8 @@ px4_add_module(
 	SRCS
 		ControlAllocator.cpp
 		ControlAllocator.hpp
+		ActuatorGroupPreflightCheck.cpp
+		ActuatorGroupPreflightCheck.hpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS

src/modules/control_allocator/ControlAllocator.cpp

@@ -380,6 +380,9 @@ ControlAllocator::Run()
 	const hrt_abstime now = hrt_absolute_time();
 	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
 
+	_actuator_group_preflight_check.handleCommand(now, _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL);
+	_actuator_group_preflight_check.updateState(now);
+
 	bool do_update = false;
 	vehicle_torque_setpoint_s vehicle_torque_setpoint;
 	vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
@@ -427,6 +430,8 @@ ControlAllocator::Run()
 			}
 		}
 
+		_actuator_group_preflight_check.applyOverrides(c, _is_vtol, *_actuator_effectiveness);
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
 			_control_allocation[i]->setControlSetpoint(c[i]);
@@ -633,6 +638,7 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 {
 	control_allocator_status_s control_allocator_status{};
 	control_allocator_status.timestamp = hrt_absolute_time();
+	control_allocator_status.actuator_group_preflight_check_active = _actuator_group_preflight_check.isActive();
 
 	// TODO: disabled motors (?)
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -55,6 +55,8 @@
 #include <ActuatorEffectivenessHelicopterCoaxial.hpp>
 #include <ActuatorEffectivenessSpacecraft.hpp>
 
+#include "ActuatorGroupPreflightCheck.hpp"
+
 #include <ControlAllocation.hpp>
 #include <ControlAllocationPseudoInverse.hpp>
 #include <ControlAllocationSequentialDesaturation.hpp>
@@ -221,6 +223,8 @@ private:
 	uint16_t _handled_motor_failure_bitmask{0};
 	uint16_t _motor_stop_mask{0};
 
+	ActuatorGroupPreflightCheck _actuator_group_preflight_check;
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};