diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f8c7f0f535..5e64b5efb9 100644 --- a/src/modules/commander/Commander.cpp +++ b/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; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 36d5c7e171..8f5dae51cf 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 + ControlSurfacePreflightCheck.cpp + ControlSurfacePreflightCheck.hpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 39642bf71d..22ed9a05d2 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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]); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 99420ee95a..ccc15d9c38 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -55,6 +55,8 @@ #include #include +#include "ControlSurfacePreflightCheck.hpp" + #include #include #include @@ -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}; diff --git a/src/modules/control_allocator/ControlSurfacePreflightCheck.cpp b/src/modules/control_allocator/ControlSurfacePreflightCheck.cpp new file mode 100644 index 0000000000..4d0a2e6f98 --- /dev/null +++ b/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 + +#include + +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 (&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); + } +} diff --git a/src/modules/control_allocator/ControlSurfacePreflightCheck.hpp b/src/modules/control_allocator/ControlSurfacePreflightCheck.hpp new file mode 100644 index 0000000000..828ea52748 --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#include +#include +#include + +/** + * 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 (&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 _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}; +};