diff --git a/msg/VehicleControlMode.msg b/msg/VehicleControlMode.msg index 1f4034369d91..92324cf03fdf 100644 --- a/msg/VehicleControlMode.msg +++ b/msg/VehicleControlMode.msg @@ -14,3 +14,4 @@ bool flag_control_position_enabled # true if position is controlled bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_allocation_enabled # true if control allocation is enabled diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 0a07c2989d95..a8b12edc5062 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -47,6 +47,7 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode_s &vehicle_control_mode) { vehicle_control_mode.flag_armed = armed; + vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default switch (nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 0a3f24df39ca..7b6009d98753 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -355,6 +355,14 @@ ControlAllocator::Run() } } + { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _publish_controls = vehicle_control_mode.flag_control_allocation_enabled; + } + } + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); @@ -637,6 +645,10 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) void ControlAllocator::publish_actuator_controls() { + if (!_publish_controls) { + return; + } + actuator_motors_s actuator_motors; actuator_motors.timestamp = hrt_absolute_time(); actuator_motors.timestamp_sample = _timestamp_sample; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index e445df72a555..de2bf887567b 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -185,10 +186,12 @@ class ControlAllocator : public ModuleBase, public ModuleParam uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; + bool _publish_controls{true}; // Reflects motor failures that are currently handled, not motor failures that are reported. // For example, the system might report two motor failures, but only the first one is handled by CA