diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fcc6a8bf48..03093f4bdd 100644 --- a/src/modules/commander/Commander.cpp +++ b/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; diff --git a/src/modules/control_allocator/ActuatorGroupPreflightCheck.cpp b/src/modules/control_allocator/ActuatorGroupPreflightCheck.cpp new file mode 100644 index 0000000000..e418120606 --- /dev/null +++ b/src/modules/control_allocator/ActuatorGroupPreflightCheck.cpp @@ -0,0 +1,242 @@ +/**************************************************************************** + * + * 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 + // - on VTOLs, thrust and control surfaces are handled by different allocator instances + 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"; + } + + // Transient: arming state mismatch. The user can change it and retry. + ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + actuator_armed_s actuator_armed{}; + _armed_sub.copy(&actuator_armed); + + if (isThrust(group)) { + + if (!actuator_armed.armed) { + return "thrust test requires armed"; + } + + } else { + + if (!actuator_armed.prearmed && !actuator_armed.armed) { + return "not (pre)armed"; + } + } + + // Transient: landed state mismatch + vehicle_land_detected_s vehicle_land_detected{}; + _vehicle_land_detected_sub.copy(&vehicle_land_detected); + + if (!vehicle_land_detected.landed) { + return "not landed"; + } + + 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); + + // 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; + + const float input = math::constrain(vehicle_command.param2, min_input, 1.f); + + 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 affecting any running check. + sendAck(vehicle_command, reject_result, now); + return; + } + + if (_running) { + // Cancel the previous check, still targeted at its original requester. + stop(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now); + } + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + start(now, group, input, vehicle_status.nav_state, vehicle_command); +} + +void ActuatorGroupPreflightCheck::updateState(hrt_abstime now) +{ + if (!_running) { return; } + + actuator_armed_s actuator_armed{}; + vehicle_land_detected_s vehicle_land_detected{}; + vehicle_status_s vehicle_status{}; + + if (_armed_sub.copy(&actuator_armed) + && _vehicle_land_detected_sub.copy(&vehicle_land_detected) + && _vehicle_status_sub.copy(&vehicle_status)) { + + // Cancel if any of the conditions we depend on are lost (thrust -> armed, + // torque/tilt -> pre-armed or armed, always !landed), or when nav_state + // changes (safety measure). + const bool is_thrust = isThrust(_active_check.group); + const bool armed_requirement_lost = is_thrust && !actuator_armed.armed; + const bool prearmed_requirement_lost = !is_thrust && (!actuator_armed.prearmed && !actuator_armed.armed); + const bool nav_state_changed = vehicle_status.nav_state != _active_check.started_nav_state; + + if (armed_requirement_lost || prearmed_requirement_lost || !vehicle_land_detected.landed || nav_state_changed) { + stop(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now); + return; + } + } + + if (now >= _active_check.started + PREFLIGHT_CHECK_DURATION_US) { + stop(vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, now); + } +} + +void ActuatorGroupPreflightCheck::applyOverrides(matrix::Vector (&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 (_active_check.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, _active_check.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(_active_check.group)) ? 1 : 0; + c[instance](override_index) = _active_check.input; + } +} + +void ActuatorGroupPreflightCheck::start(hrt_abstime now, uint8_t group, float input, uint8_t nav_state, + const vehicle_command_s &cmd) +{ + _active_check = {group, input, now, nav_state, cmd}; + _running = true; + sendAck(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS, now); +} + +void ActuatorGroupPreflightCheck::stop(uint8_t result, hrt_abstime now) +{ + _running = false; + sendAck(_active_check.last_command, result, now); +} + +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); + } +} diff --git a/src/modules/control_allocator/ActuatorGroupPreflightCheck.hpp b/src/modules/control_allocator/ActuatorGroupPreflightCheck.hpp new file mode 100644 index 0000000000..b8f1a63def --- /dev/null +++ b/src/modules/control_allocator/ActuatorGroupPreflightCheck.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/** + * 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. + * + * Pre-conditions (enforced at start, cancelled if not given while running): + * - torque and collective tilt groups: prearmed or armed + * - thrust groups: armed + * - landed + * + * 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 (&c)[MAX_NUM_MATRICES], bool is_vtol, + ActuatorEffectiveness &effectiveness); + +private: + static constexpr hrt_abstime PREFLIGHT_CHECK_DURATION_US = 500'000; // 500 ms + + 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); + + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + void start(hrt_abstime now, uint8_t group, float input, uint8_t nav_state, const vehicle_command_s &cmd); + void stop(uint8_t result, hrt_abstime now); + void sendAck(const vehicle_command_s &cmd, uint8_t result, hrt_abstime now); + + bool _running{false}; + struct ActiveCheck { + uint8_t group; + float input; + hrt_abstime started; + uint8_t started_nav_state; + vehicle_command_s last_command; + } _active_check{}; +}; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 36d5c7e171..20ac64dc7d 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/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 diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 7f3abfe366..687d31e972 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -371,6 +371,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; @@ -418,6 +421,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]); @@ -624,6 +629,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 (?) diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 46129ec4ad..a8ebb762f3 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -54,6 +54,8 @@ #include #include +#include "ActuatorGroupPreflightCheck.hpp" + #include #include #include @@ -220,6 +222,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};