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

CA: handle servo failure detection

Browse Source 
- add fd_servo to failure detector status
- inside control allocation: if one axis is no longer controllable with servos
then enable auxiliary motors for it's control (VTOL only)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-10 11:16:32 +02:00
parent d27ac8a2cb
commit 1fcd5b466f
6 changed files with 82 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/FailureDetectorStatus.msg
+2
View File
@@ -9,6 +9,8 @@ bool fd_arm_escs
bool fd_battery
bool fd_imbalanced_prop
bool fd_motor
bool fd_servo
float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed)
uint16 motor_failure_mask # Bit-mask with motor indices, indicating critical motor failures
uint16 servo_failure_mask # Bit-mask with servo indices, indicating critical servo failures
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
-10
View File
@@ -98,15 +98,6 @@ 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
*
@@ -244,6 +235,5 @@ protected:
matrix::Vector<float, NUM_AXES> _control_trim; ///< Control at trim actuator values
int _num_actuators{0};
bool _normalize_matrix_as_planar_mc{false}; ///< if true, normalize roll, pitch and yaw columns optimized for planar MC
bool _had_actuator_failure{false};
int _airmode{0}; ///< 0: disabled, 1: RP airmode, 2: RPY airmode
};
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
+1 -1
View File
@@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
if (_mix_update_needed) {
matrix::geninv(_effectiveness, _mix);
if (_normalization_needs_update && !_had_actuator_failure) {
if (_normalization_needs_update) {
updateControlAllocationMatrixScale();
_normalization_needs_update = false;
}
src/modules/control_allocator/ControlAllocator.cpp
+60 -14
View File
@@ -419,6 +419,14 @@ ControlAllocator::Run()
fw_dthr_scale[0] = fw_dthr_scale[2];
}
for (int i = 0; i < 3; i++) {
if (_handled_servo_failure_bitmask && _param_ca_fw_dthr_fb_en.get() && !_has_control_authority[1][i]) {
// If fallback is enabled and it is discovered that an axis doesn't have any
// control authority, then the differential thrust weight for that axis is set to 1.
fw_dthr_weight[i] = 1.f;
}
}
// Set control setpoint vector(s)
matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
c[0](0) = _torque_sp(0);
@@ -451,11 +459,8 @@ ControlAllocator::Run()
c[1](5) = vehicle_thrust_setpoint.xyz[2];
}
const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON
|| fw_dthr_weight[2] > FLT_EPSILON;
// VTOL differential thrust in FW
if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && has_non_zero_dthr_weight) {
if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT) {
/* CA_FW_DTHR_SC_R etc are scales to tune response of differential thrust around each axis.
The scaling factor is applied to the (normalized) torque setpoint from the rate controller going
to the motors for rate control in fixed-wing flight using differential thrust instead of
@@ -469,10 +474,16 @@ ControlAllocator::Run()
c[0](1) = vehicle_torque_setpoint_matrix_1(1) * fw_dthr_scale[1] * fw_dthr_weight[1];
c[0](2) = vehicle_torque_setpoint_matrix_1(2) * fw_dthr_scale[2] * fw_dthr_weight[2];
_actuator_effectiveness->setEnableAuxiliaryMotors(true);
const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON
|| fw_dthr_weight[2] > FLT_EPSILON;
} else if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && !has_non_zero_dthr_weight) {
_actuator_effectiveness->setEnableAuxiliaryMotors(false);
if (has_non_zero_dthr_weight) {
_actuator_effectiveness->setEnableAuxiliaryMotors(true); // Currently only does something for Standard VTOL
} else {
_actuator_effectiveness->setEnableAuxiliaryMotors(false);
}
}
}
@@ -612,6 +623,27 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
}
}
// Handle failed servos
if (_handled_servo_failure_bitmask) {
actuator_idx = 0;
memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix));
for (int servos_idx = 0; servos_idx < _num_actuators[0] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) {
const int selected_matrix = 1; // matrix 1 (only works for VTOL atm)
if (_handled_servo_failure_bitmask & (1 << servos_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.f;
}
}
++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]);
@@ -634,6 +666,10 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
if (all_entries_small) {
matrix.row(n) = 0.f;
_has_control_authority[i][n] = false;
} else {
_has_control_authority[i][n] = true;
}
}
@@ -773,8 +809,10 @@ ControlAllocator::check_for_actuator_activation_update()
failure_detector_status_s failure_detector_status;
const bool status_updated = _failure_detector_status_sub.update(&failure_detector_status);
if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
&& _failure_detector_status_sub.update(&failure_detector_status)) {
&& status_updated) {
if (failure_detector_status.fd_motor) {
if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) {
@@ -789,10 +827,6 @@ ControlAllocator::check_for_actuator_activation_update()
_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);
}
}
}
break;
@@ -808,10 +842,22 @@ ControlAllocator::check_for_actuator_activation_update()
PX4_INFO("Restoring all motors");
_handled_motor_failure_bitmask = 0;
for (int i = 0; i < _num_control_allocation; ++i) {
_control_allocation[i]->setHadActuatorFailure(false);
activation_updated = true;
}
if (failure_detector_status.fd_servo) {
if (_handled_servo_failure_bitmask != failure_detector_status.servo_failure_mask) {
// servo failure bitmask changed
_handled_servo_failure_bitmask = failure_detector_status.servo_failure_mask;
PX4_WARN("Removing servo nr. %d from allocation", _handled_servo_failure_bitmask);
activation_updated = true;
}
} else if (_handled_servo_failure_bitmask != 0) {
// Clear bitmask completely
PX4_INFO("Restoring all servos");
_handled_servo_failure_bitmask = 0;
activation_updated = true;
}
}
src/modules/control_allocator/ControlAllocator.hpp
+5 -1
View File
@@ -198,6 +198,7 @@ private:
// 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};
uint16_t _handled_servo_failure_bitmask{0};
uint16_t _handled_motor_disabled_bitmask{0};
@@ -213,6 +214,8 @@ private:
bool _has_slew_rate{false};
ActuatorEffectiveness::FlightPhase _flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
bool _has_control_authority[2][6] = {false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
@@ -225,7 +228,8 @@ private:
(ParamFloat<px4::params::CA_FW_DTHR_WGT_P>) _param_ca_fw_dthr_wgt_p,
(ParamFloat<px4::params::CA_FW_DTHR_WGT_Y>) _param_ca_fw_dthr_wgt_y,
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
(ParamInt<px4::params::CA_FW_DTHR_AIRMD>) _param_ca_fw_dthr_airmd
(ParamInt<px4::params::CA_FW_DTHR_AIRMD>) _param_ca_fw_dthr_airmd,
(ParamBool<px4::params::CA_FW_DTHR_FB_EN>) _param_ca_fw_dthr_fb_en
)
};
src/modules/control_allocator/module.yaml
+14
View File
@@ -619,6 +619,20 @@ parameters:
1: Roll/Pitch
2: Roll/Pitch/Yaw
default: 0
CA_FW_DTHR_FB_EN:
description:
short: Enable fallback to differential thrust for control in fixed-wing
long: |
Enable fallback to differential thrust for rate control around axes that have
0 effectiveness in the servo matrix after the detected servo failure.
type: boolean
values:
0: Disabled
1: Enabled
default: 1
# Mixer
mixer:
actuator_types: