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};