fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

CA: Differentiate between STOPPED and DISABLED actuator

Browse Source 
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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-05-27 18:37:47 +02:00
parent 41393f0618
commit 1695babe8a
11 changed files with 104 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp
View File

@ -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);

26
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
View File

@ -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<float, NUM_ACTUATORS> &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};
};

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp
View File

@ -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<float, 6> control_sp{};

16
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
View File

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

1
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
View File

@ -80,6 +80,7 @@ public:
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void setFlightPhase(const FlightPhase &flight_phase) override;
void setEnableAuxiliaryMotors(bool enable) override;
private:
ActuatorEffectivenessRotors _rotors;

15
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
View File

@ -93,7 +93,13 @@ void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<fl
const matrix::Vector<float, NUM_ACTUATORS> &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
}

1
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp
View File

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

21
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -201,14 +201,20 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
// in FW directly use throttle sp
if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
for (int i = 0; i < _first_tilt_idx; ++i) {
actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
for (int motors_idx = 0; motors_idx < _first_tilt_idx; ++motors_idx) {
if (!(_disabled_motors_mask & (1 << motors_idx))) {
actuator_sp(motors_idx) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
}
}
}
}
if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
} else {
// make sure all motors are un-stopped when out of forward flight
stopMaskedMotorsWithZeroThrust(0, actuator_sp);
}
}
}
@ -221,20 +227,25 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
ActuatorEffectiveness::setFlightPhase(flight_phase);
// update stopped motors
// update disabled motors
switch (flight_phase) {
case FlightPhase::FORWARD_FLIGHT:
_stopped_motors_mask |= _untiltable_motors;
_disabled_motors_mask |= _untiltable_motors;
break;
case FlightPhase::HOVER_FLIGHT:
case FlightPhase::TRANSITION_FF_TO_HF:
case FlightPhase::TRANSITION_HF_TO_FF:
_stopped_motors_mask = 0;
_disabled_motors_mask = 0;
break;
}
}
void ActuatorEffectivenessTiltrotorVTOL::setEnableAuxiliaryMotors(bool enable)
{
// keep auxiliary motors disabled for now
}
void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
{
// only handle matrix 0 (motors and tilts)

1
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
View File

@ -77,6 +77,7 @@ public:
}
void setFlightPhase(const FlightPhase &flight_phase) override;
void setEnableAuxiliaryMotors(bool enable) override;
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;

41
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -400,9 +400,13 @@ ControlAllocator::Run()
if (do_update) {
_last_run = now;
check_for_motor_failures();
// update matrix if there was an actuator activation change (due to detected failure or actuator activation due to VTOL mode change)
if (check_for_actuator_activation_update()) {
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::ACTUATOR_ACTIVATION_UPDATE);
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
} else {
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
}
// Set control setpoint vector(s)
matrix::Vector<float, NUM_AXES> 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[])

4
src/modules/control_allocator/ControlAllocator.hpp
View File

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