feat(control_allocator): Add control surface 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`
(https://github.com/mavlink/mavlink/pull/2224).

Only runs in pre-armed state (COM_PREARM_MODE > 0).

src/modules/commander/Commander.cpp

@@ -1650,6 +1650,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/CMakeLists.txt

@@ -44,6 +44,8 @@ px4_add_module(
 	SRCS
 		ControlAllocator.cpp
 		ControlAllocator.hpp
+		ControlSurfacePreflightCheck.cpp
+		ControlSurfacePreflightCheck.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);
 
+	_cs_preflight_check.handleCommand(now, _armed,
+					  _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL);
+
 	bool do_update = false;
 	vehicle_torque_setpoint_s vehicle_torque_setpoint;
 	vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
@@ -427,6 +430,9 @@ ControlAllocator::Run()
 			}
 		}
 
+		_cs_preflight_check.updateState(now, _armed);
+		_cs_preflight_check.applyOverrides(c, _is_vtol, *_actuator_effectiveness);
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
 			_control_allocation[i]->setControlSetpoint(c[i]);

src/modules/control_allocator/ControlAllocator.hpp

@@ -55,6 +55,8 @@
 #include <ActuatorEffectivenessHelicopterCoaxial.hpp>
 #include <ActuatorEffectivenessSpacecraft.hpp>
 
+#include "ControlSurfacePreflightCheck.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};
 
+	ControlSurfacePreflightCheck _cs_preflight_check;
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};

src/modules/control_allocator/ControlSurfacePreflightCheck.cpp

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   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 "ControlSurfacePreflightCheck.hpp"
+
+#include <math.h>
+
+#include <px4_platform_common/log.h>
+
+void ControlSurfacePreflightCheck::handleCommand(hrt_abstime now, bool armed, 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 axis = (uint8_t) lroundf(vehicle_command.param1);
+	const char *reject_reason = nullptr;
+	actuator_armed_s actuator_armed;
+
+	if (armed) {
+		reject_reason = "armed";
+
+	} else if (!_armed_sub.copy(&actuator_armed)) {
+		reject_reason = "failed to get prearmed state";
+
+	} else if (!actuator_armed.prearmed) {
+		reject_reason = "not prearmed";
+
+	} else if (axis == vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT && !is_tiltrotor) {
+		reject_reason = "tilt commanded but not available";
+	}
+
+	uint8_t result;
+
+	if (reject_reason != nullptr) {
+		result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		PX4_WARN("Control surface preflight check rejected (%s)", reject_reason);
+
+	} else {
+		if (_running) {
+			// Cancel the previous check (still targeted at its original requester).
+			sendAck(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
+		}
+
+		_axis = axis;
+		_input = vehicle_command.param2;
+		_started = now;
+		_running = true;
+		result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+	}
+
+	// Store the command so sendAck() can target the originating system/component.
+	_last_command = vehicle_command;
+	sendAck(result, now);
+}
+
+void ControlSurfacePreflightCheck::updateState(hrt_abstime now, bool armed)
+{
+	if (!_running) {
+		return;
+	}
+
+	if (armed) {
+		_running = false;
+		sendAck(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
+
+	} else if (now - _started >= PREFLIGHT_CHECK_DURATION) {
+		_running = false;
+		sendAck(vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, now);
+	}
+}
+
+void ControlSurfacePreflightCheck::applyOverrides(matrix::Vector<float, NUM_AXES> (&c)[MAX_NUM_MATRICES],
+		bool is_vtol, ActuatorEffectiveness &effectiveness)
+{
+	// Clear/refresh tilt override every tick so it doesn't outlive the check
+	// or any effectiveness-source change.
+	const bool tilt_active = _running && _axis == vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT;
+	effectiveness.overrideCollectiveTilt(tilt_active, tilt_active ? _input : 0.f);
+
+	// ACTUATOR_TEST_GROUP_{ROLL,PITCH,YAW}_TORQUE are 0/1/2 - used directly as torque-vector indices.
+	if (!_running || _axis > vehicle_command_s::ACTUATOR_TEST_GROUP_YAW_TORQUE) {
+		return;
+	}
+
+	// On a VTOL, instance 1 drives the fixed-wing surfaces; otherwise instance 0.
+	const int instance = is_vtol ? 1 : 0;
+
+	c[instance](0) = 0.f;
+	c[instance](1) = 0.f;
+	c[instance](2) = 0.f;
+	c[instance](_axis) = _input;
+}
+
+void ControlSurfacePreflightCheck::sendAck(uint8_t result, hrt_abstime now)
+{
+	if (_last_command.from_external) {
+		vehicle_command_ack_s command_ack{};
+		command_ack.timestamp = now;
+		command_ack.command = _last_command.command;
+		command_ack.result = result;
+		command_ack.target_system = _last_command.source_system;
+		command_ack.target_component = _last_command.source_component;
+		_command_ack_pub.publish(command_ack);
+	}
+}

src/modules/control_allocator/ControlSurfacePreflightCheck.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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 <drivers/drv_hrt.h>
+#include <lib/matrix/matrix/math.hpp>
+#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 (or collective tilt) on a single axis for a short duration so the
+ * operator can verify control-surface motion before flight. Runs only while
+ * disarmed.
+ */
+class ControlSurfacePreflightCheck
+{
+public:
+	static constexpr int NUM_AXES = ControlAllocation::NUM_AXES;
+	static constexpr int MAX_NUM_MATRICES = ActuatorEffectiveness::MAX_NUM_MATRICES;
+
+	ControlSurfacePreflightCheck() = default;
+	~ControlSurfacePreflightCheck() = default;
+
+	/** Poll vehicle_command for a preflight-check request and start it if conditions allow. */
+	void handleCommand(hrt_abstime now, bool armed, bool is_tiltrotor);
+
+	/** Finish the check after PREFLIGHT_CHECK_DURATION, or abort if armed mid-check. */
+	void updateState(hrt_abstime now, bool armed);
+
+	/**
+	 * Apply the test torque setpoint and/or collective-tilt override for this tick.
+	 * Always called so the tilt override is cleared once the check ends.
+	 */
+	void applyOverrides(matrix::Vector<float, NUM_AXES> (&c)[MAX_NUM_MATRICES], bool is_vtol,
+			    ActuatorEffectiveness &effectiveness);
+
+private:
+	static constexpr uint32_t PREFLIGHT_CHECK_DURATION = 500'000;  // 500 ms
+
+	void sendAck(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};
+	uint8_t _axis{0};
+	float _input{0.0f};
+	vehicle_command_s _last_command{};
+	hrt_abstime _started{0};
+};