diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg index 2e90dbd454..3fbbe844ef 100644 --- a/msg/control_allocator_status.msg +++ b/msg/control_allocator_status.msg @@ -19,3 +19,5 @@ int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a valu 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 diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 0d7895374e..cc6d76486c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -98,6 +98,15 @@ public: */ virtual void allocate() = 0; + /** + * Set actuator failure flag + * This prevents a change of the scaling in the matrix normalization step + * in case of a motor failure. + * + * @param failure Motor failure flag + */ + void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; } + /** * Set the control effectiveness matrix * @@ -234,4 +243,5 @@ protected: matrix::Vector _control_trim; ///< Control at trim actuator values int _num_actuators{0}; bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns + bool _had_actuator_failure{false}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 65fd35d98e..ee1d2dceb2 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse() if (_mix_update_needed) { matrix::geninv(_effectiveness, _mix); - if (_normalization_needs_update) { + if (_normalization_needs_update && !_had_actuator_failure) { updateControlAllocationMatrixScale(); _normalization_needs_update = false; } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 6bfe493d5a..83be2a3b33 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -92,8 +92,8 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act float k_max = 0.f; for (int i = 0; i < _num_actuators; i++) { - // Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway - if (fabsf(desaturation_vector(i)) < FLT_EPSILON) { + // Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains + if (fabsf(desaturation_vector(i)) < 0.2f) { continue; } diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 70e7e8b695..72451011f9 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -205,7 +205,7 @@ ControlAllocator::update_allocation_method(bool force) bool ControlAllocator::update_effectiveness_source() { - EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); + const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); if (_effectiveness_source_id != source) { @@ -305,8 +305,12 @@ ControlAllocator::Run() parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); - updateParams(); - parameters_updated(); + if (_handled_motor_failure_bitmask == 0) { + // We don't update the geometry after an actuator failure, as it could lead to unexpected results + // (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore) + updateParams(); + parameters_updated(); + } } if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) { @@ -376,6 +380,8 @@ ControlAllocator::Run() if (do_update) { _last_run = now; + check_for_motor_failures(); + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE); // Set control setpoint vector(s) @@ -503,6 +509,27 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso } } + // Handle failed actuators + if (_handled_motor_failure_bitmask) { + actuator_idx = 0; + memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix)); + + for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + + if (_handled_motor_failure_bitmask & (1 << motors_idx)) { + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix]; + + for (int i = 0; i < NUM_AXES; i++) { + matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f; + } + } + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + } + for (int i = 0; i < _num_control_allocation; ++i) { _control_allocation[i]->setActuatorMin(minimum[i]); _control_allocation[i]->setActuatorMax(maximum[i]); @@ -557,7 +584,8 @@ ControlAllocator::publish_control_allocator_status() control_allocator_status.allocated_thrust[2] = allocated_control(5); // Unallocated control - matrix::Vector unallocated_control = _control_allocation[0]->getControlSetpoint() - allocated_control; + const matrix::Vector unallocated_control = _control_allocation[0]->getControlSetpoint() - + allocated_control; control_allocator_status.unallocated_torque[0] = unallocated_control(0); control_allocator_status.unallocated_torque[1] = unallocated_control(1); control_allocator_status.unallocated_torque[2] = unallocated_control(2); @@ -585,6 +613,9 @@ ControlAllocator::publish_control_allocator_status() } } + // Handled motor failures + control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask; + _control_allocator_status_pub.publish(control_allocator_status); } @@ -604,7 +635,7 @@ ControlAllocator::publish_actuator_controls() int actuator_idx = 0; int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; - uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors(); + uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; // motors int motors_idx; @@ -648,6 +679,56 @@ ControlAllocator::publish_actuator_controls() } } +void +ControlAllocator::check_for_motor_failures() +{ + failure_detector_status_s failure_detector_status; + + if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE + && _failure_detector_status_sub.update(&failure_detector_status)) { + if (failure_detector_status.fd_motor) { + + if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) { + // motor failure bitmask changed + switch ((FailureMode)_param_ca_failure_mode.get()) { + case FailureMode::REMOVE_FIRST_FAILING_MOTOR: { + // Count number of failed motors + const int num_motors_failed = math::countSetBits(failure_detector_status.motor_failure_mask); + + // Only handle if it is the first failure + if (_handled_motor_failure_bitmask == 0 && num_motors_failed == 1) { + _handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask; + PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask); + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setHadActuatorFailure(true); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + } + } + break; + + default: + break; + } + + } + + } else if (_handled_motor_failure_bitmask != 0) { + // Clear bitmask completely + PX4_INFO("Restoring all motors"); + _handled_motor_failure_bitmask = 0; + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setHadActuatorFailure(false); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + } + } +} + int ControlAllocator::task_spawn(int argc, char *argv[]) { ControlAllocator *instance = new ControlAllocator(); @@ -716,6 +797,11 @@ int ControlAllocator::print_status() PX4_INFO(" Configured actuators: %i", _control_allocation[i]->numConfiguredActuators()); } + if (_handled_motor_failure_bitmask) { + PX4_INFO("Failed motors: %i (0x%x)", math::countSetBits(_handled_motor_failure_bitmask), + _handled_motor_failure_bitmask); + } + // Print perf perf_print_counter(_loop_perf); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 2942dd9fab..1b016dd331 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -74,6 +74,7 @@ #include #include #include +#include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -128,6 +129,8 @@ private: void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason); + void check_for_motor_failures(); + void publish_control_allocator_status(); void publish_actuator_controls(); @@ -152,6 +155,11 @@ private: HELICOPTER = 10, }; + enum class FailureMode { + IGNORE = 0, + REMOVE_FIRST_FAILING_MOTOR = 1, + }; + EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness @@ -175,10 +183,15 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; + // 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 + uint16_t _handled_motor_failure_bitmask{0}; + perf_counter_t _loop_perf; /**< loop duration performance counter */ bool _armed{false}; @@ -193,6 +206,7 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, + (ParamInt) _param_ca_failure_mode, (ParamInt) _param_r_rev ) diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index c1f64e35cd..34618b3877 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -436,6 +436,19 @@ parameters: max: 1 default: [0.05, 0.15, 0.25, 0.35, 0.45] + # Others + CA_FAILURE_MODE: + description: + short: Motor failure handling mode + long: | + This is used to specify how to handle motor failures + reported by failure detector. + type: enum + values: + 0: Ignore + 1: Remove first failed motor from effectiveness + default: 0 + # Mixer mixer: actuator_types: