feat(control_allocator): reject actuator group check in flight

according to the land detector. torque / tilt are rejected anyway
because they require pre-armed && !armed, but thrust could previously be
triggered in flight.
This commit is contained in:
Balduin
2026-05-07 08:44:13 +02:00
parent 5b07e0aa2f
commit afbc2e97ff
2 changed files with 21 additions and 4 deletions
@@ -83,6 +83,12 @@ const char *ActuatorGroupPreflightCheck::validateCommand(uint8_t group, bool is_
return "failed to read armed state";
}
vehicle_land_detected_s vehicle_land_detected;
if (!_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
return "failed to read landed state";
}
// Transient: arming state mismatch. The user can change it and retry.
ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
@@ -103,6 +109,11 @@ const char *ActuatorGroupPreflightCheck::validateCommand(uint8_t group, bool is_
}
}
// Transient: landed state mismatch
if (!vehicle_land_detected.landed) {
return "not landed";
}
return nullptr;
}
@@ -150,14 +161,16 @@ void ActuatorGroupPreflightCheck::updateState(hrt_abstime now)
if (!_running) { return; }
actuator_armed_s actuator_armed;
vehicle_land_detected_s vehicle_land_detected;
if (_armed_sub.copy(&actuator_armed) && _vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
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).
// (thrust -> armed, torque/tilt -> pre-armed, always !landed).
const bool armed_lost = actuator_armed.armed != _requires_armed;
const bool prearmed_lost = !_requires_armed && !actuator_armed.prearmed;
if (armed_lost || prearmed_lost) {
if (armed_lost || prearmed_lost || !vehicle_land_detected.landed) {
_running = false;
sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
return;
@@ -44,18 +44,21 @@
#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>
#include <uORB/topics/vehicle_land_detected.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):
* Pre-conditions (enforced at start, cancelled if not given while running):
* - torque and collective-tilt groups: prearmed and *not* armed
* - thrust groups: armed
* - landed
*
* Losing the required state mid-check results in a CANCELLED ack.
*/
@@ -96,6 +99,7 @@ private:
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::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
bool _running{false};