From 1695babe8a9426d65d6aef7fc7e17b2d8b470ae2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 27 May 2024 18:37:47 +0200 Subject: [PATCH] CA: Differentiate between STOPPED and DISABLED actuator STOPPED: actuators are NOT remoted from the allocation, but output is set to NAN. Intended for completely stopping a motor that anyway already had a very low thrust setpoint. DISABLED: Remove actuator from effectiveness matrix and set output to NAN. Intended to tell the allocation when an actuator shouldn't be used in the current flight state to allocate torque. Additionally added a method to enable/disable motors that are considered auxiliary (e.g. hover motors in FW flight on a Standard VTOL). Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectiveness.cpp | 2 + .../ActuatorEffectiveness.hpp | 26 +++++++++++- .../ActuatorEffectivenessHelicopterTest.cpp | 2 +- .../ActuatorEffectivenessStandardVTOL.cpp | 16 ++++++-- .../ActuatorEffectivenessStandardVTOL.hpp | 1 + .../ActuatorEffectivenessTailsitterVTOL.cpp | 15 ++++++- .../ActuatorEffectivenessTailsitterVTOL.hpp | 1 + .../ActuatorEffectivenessTiltrotorVTOL.cpp | 21 +++++++--- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 1 + .../control_allocator/ControlAllocator.cpp | 41 +++++++++++++------ .../control_allocator/ControlAllocator.hpp | 4 +- 11 files changed, 104 insertions(+), 26 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp index 901c469746..b74f6232e5 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -87,6 +87,8 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) { + _stopped_motors_mask = 0; // TODO: as we have to reset here every iteration, there is no need to store this in the class + for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) { const uint32_t motor_mask = (1u << actuator_idx); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 3dfea4f75f..77d6a7db1e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -63,7 +63,7 @@ enum class ActuatorType { enum class EffectivenessUpdateReason { NO_EXTERNAL_UPDATE = 0, CONFIGURATION_UPDATE = 1, - MOTOR_ACTIVATION_UPDATE = 2, + ACTUATOR_ACTIVATION_UPDATE = 2, }; @@ -134,6 +134,17 @@ public: _flight_phase = flight_phase; } + /** + * Set flag to enable/disable auxiliary motors (effectiveness-type specific) + * + * Auxiliary motors are switched off (stopped) by default, but can be enabled + * in certain cases (e.g. MC motors on a Standard VTOL in forward flight for + * attitude control support). + * + * @param enable True to enable auxiliary motors, false to disable them. + */ + virtual void setEnableAuxiliaryMotors(bool enable) {} + /** * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES. * This is expected to stay constant. @@ -201,10 +212,20 @@ public: const matrix::Vector &actuator_max) {} /** - * Get a bitmask of motors to be stopped + * Get a bitmask of motors to be stopped. + * Stopped motors are by definition NOT to be removed from the control allocation matrix, + * but their output is to be stopped (setpoint=NAN). + * Usually is used to not have motors spinning at 0 thrust. */ virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; } + /** + * Get a bitmask of motors to be disabled. + * Disabled motors are by definition to be removed from the control allocation matrix and + * their output is to be stopped (setpoint=NAN). + */ + virtual uint32_t getDisabledMotors() const { return _disabled_motors_mask; } + /** * Fill in the unallocated torque and thrust, customized by effectiveness type. * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead. @@ -222,4 +243,5 @@ public: protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; uint32_t _stopped_motors_mask{0}; + uint32_t _disabled_motors_mask{0}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp index 29ca1f1117..30016b3744 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp @@ -55,7 +55,7 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve) ActuatorEffectivenessHelicopter helicopter(nullptr, ActuatorType::MOTORS); // run getEffectivenessMatrix with empty configuration to correctly initialize _first_swash_plate_servo_index ActuatorEffectiveness::Configuration configuration{}; - EffectivenessUpdateReason external_update = EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE; + EffectivenessUpdateReason external_update = EffectivenessUpdateReason::ACTUATOR_ACTIVATION_UPDATE; helicopter.getEffectivenessMatrix(configuration, external_update); Vector control_sp{}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 79853c7518..07651e315b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -101,16 +101,26 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors + // update disabled motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors_mask |= _upwards_motors_mask; + _disabled_motors_mask |= _upwards_motors_mask; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors_mask &= ~_upwards_motors_mask; + _disabled_motors_mask &= ~_upwards_motors_mask; break; } } + +void ActuatorEffectivenessStandardVTOL::setEnableAuxiliaryMotors(bool enable) +{ + if (enable) { + _disabled_motors_mask = 0; + + } else { + _disabled_motors_mask = _upwards_motors_mask; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 988f102474..afa68586ba 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -80,6 +80,7 @@ public: const matrix::Vector &actuator_max) override; void setFlightPhase(const FlightPhase &flight_phase) override; + void setEnableAuxiliaryMotors(bool enable) override; private: ActuatorEffectivenessRotors _rotors; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index aad6d4391b..776cdc6a93 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -93,7 +93,13 @@ void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &actuator_max) { if (matrix_index == 0) { - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + + } else { + // make sure all motors are un-stopped when out of forward flight + stopMaskedMotorsWithZeroThrust(0, actuator_sp); + } } } @@ -115,7 +121,12 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: _forwards_motors_mask = 0; - _stopped_motors_mask = 0; + _disabled_motors_mask = 0; break; } } + +void ActuatorEffectivenessTailsitterVTOL::setEnableAuxiliaryMotors(bool enable) +{ + // keep auxiliary motors disabled for now +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index bb25c931ba..24268da65e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -78,6 +78,7 @@ public: void setFlightPhase(const FlightPhase &flight_phase) override; + void setEnableAuxiliaryMotors(bool enable) override; const char *name() const override { return "VTOL Tailsitter"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 5fe261b27f..51d1ae041a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -201,14 +201,20 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector c[ActuatorEffectiveness::MAX_NUM_MATRICES]; @@ -539,15 +543,17 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso } } - // Handle failed actuators - if (_handled_motor_failure_bitmask) { + const int16_t motor_failed_disabled_bitmask = _handled_motor_failure_bitmask | _handled_motor_disabled_bitmask; + + // Handle failed/disabled motors + if (motor_failed_disabled_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)) { + if (motor_failed_disabled_bitmask & (1 << motors_idx)) { ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix]; for (int i = 0; i < NUM_AXES; i++) { @@ -668,7 +674,9 @@ ControlAllocator::publish_actuator_controls() int actuator_idx = 0; int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; - uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; + // set output to NAN of motors that are stopped or disabled + const uint32_t stopped_or_disabled_motors = _actuator_effectiveness->getStoppedMotors() | + _actuator_effectiveness->getDisabledMotors(); // motors int motors_idx; @@ -678,7 +686,7 @@ ControlAllocator::publish_actuator_controls() float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; - if (stopped_motors & (1u << motors_idx)) { + if (stopped_or_disabled_motors & (1u << motors_idx)) { actuator_motors.control[motors_idx] = NAN; } @@ -712,9 +720,11 @@ ControlAllocator::publish_actuator_controls() } } -void -ControlAllocator::check_for_motor_failures() +bool +ControlAllocator::check_for_actuator_activation_update() { + bool activation_updated = false; + failure_detector_status_s failure_detector_status; if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE @@ -732,12 +742,11 @@ ControlAllocator::check_for_motor_failures() 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); + activation_updated = true; for (int i = 0; i < _num_control_allocation; ++i) { _control_allocation[i]->setHadActuatorFailure(true); } - - update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); } } break; @@ -757,9 +766,17 @@ ControlAllocator::check_for_motor_failures() _control_allocation[i]->setHadActuatorFailure(false); } - update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + activation_updated = true; } } + + // handle disabled actuators (currently only VTOL motors) + if (_actuator_effectiveness->getDisabledMotors() != _handled_motor_disabled_bitmask) { + _handled_motor_disabled_bitmask = _actuator_effectiveness->getDisabledMotors(); + activation_updated = true; + } + + return activation_updated; } int ControlAllocator::task_spawn(int argc, char *argv[]) diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index fb17dd68b8..925f95a2d8 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -132,7 +132,7 @@ private: void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason); - void check_for_motor_failures(); + bool check_for_actuator_activation_update(); void publish_control_allocator_status(int matrix_index); @@ -199,6 +199,8 @@ private: // 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}; + uint16_t _handled_motor_disabled_bitmask{0}; + perf_counter_t _loop_perf; /**< loop duration performance counter */ bool _armed{false};