feat(control_allocator): expose cs_preflight_check_active in ControlAllocatorStatus

So that we know from logs if the torque setpoint topic is being used or
overridden by the preflight check.
This commit is contained in:
Balduin
2026-04-28 13:35:19 +02:00
parent 97547052c2
commit 8d013a6552
3 changed files with 7 additions and 2 deletions
+4 -2
View File
@@ -18,5 +18,7 @@ int8[16] actuator_saturation # Indicates actuator saturation status.
# Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
# Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.
uint16 handled_motor_failure_mask # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector
uint16 motor_stop_mask # Bitmaks of motors stopped by failure injection
uint16 handled_motor_failure_mask # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector
uint16 motor_stop_mask # Bitmaks of motors stopped by failure injection
bool cs_preflight_check_active # True while a control-surface preflight check (VEHICLE_CMD_ACTUATOR_GROUP_TEST) is overriding the torque setpoint or collective-tilt
@@ -639,6 +639,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.cs_preflight_check_active = _cs_preflight_check.isActive();
// TODO: disabled motors (?)
@@ -59,6 +59,8 @@ public:
ControlSurfacePreflightCheck() = default;
~ControlSurfacePreflightCheck() = default;
bool isActive() const { return _running; }
/** Poll vehicle_command for a preflight-check request and start it if conditions allow. */
void handleCommand(hrt_abstime now, bool armed, bool is_tiltrotor);