diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 3b44f7cdb5..4eff65a1ad 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -481,6 +481,10 @@ ControlAllocator::Run() void ControlAllocator::preflight_check_handle_command(hrt_abstime now) { + + // Only start the check if prearmed but not armed. Otherwise, output a + // warning message and reject the command. + vehicle_command_s vehicle_command; if (_vehicle_command_sub.update(&vehicle_command)) { @@ -489,13 +493,30 @@ void ControlAllocator::preflight_check_handle_command(hrt_abstime now) if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK) { if (!_armed) { - // currently this does not check prearmed status. if not prearmed, it will just do nothing. - preflight_check_start(vehicle_command, now); - result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; + + actuator_armed_s armed; + + if (_armed_sub.copy(&armed)) { + + const bool prearmed = armed.prearmed; + + if (prearmed) { + // currently this does not check prearmed status. if not prearmed, it will just do nothing. + preflight_check_start(vehicle_command, now); + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; + } else { + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + PX4_WARN("Control surface preflight check rejected (not prearmed)"); + } + + } else { + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + PX4_WARN("Control surface preflight check rejected (failed to get prearmed state)"); + } } else { result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - PX4_INFO("Control surface preflight check rejected (armed)"); + PX4_WARN("Control surface preflight check rejected (armed)"); } preflight_check_send_ack(result, now); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index c670e60918..45d6256413 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -68,6 +68,8 @@ #include #include #include + +#include #include #include #include @@ -197,6 +199,7 @@ private: uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */ uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; // Outputs uORB::PublicationMulti _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};