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

CA testing hacks, external failure message, sitl tuning, disabling of rate controller saturation

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-05-27 18:04:50 +02:00
parent 94e98c2457
commit 619812cbdd
13 changed files with 251 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+35
View File
@@ -644,6 +644,22 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
}
}
// handle servo failure injection
if (_handled_servo_center_mask) {
if (_handled_servo_center_mask & 1) {
// set the first servo to 0 by setting the min/max to 0 (center)
minimum[1](0) = 0.f;
maximum[1](0) = 0.f;
}
if (_handled_servo_center_mask & 2) {
// set the second servo to 0 by setting the min/max to 0 (center)
minimum[1](1) = 0.f;
maximum[1](1) = 0.f;
}
}
for (int i = 0; i < _num_control_allocation; ++i) {
_control_allocation[i]->setActuatorMin(minimum[i]);
_control_allocation[i]->setActuatorMax(maximum[i]);
@@ -818,6 +834,25 @@ ControlAllocator::check_for_actuator_activation_update()
const bool status_updated = _failure_detector_status_sub.update(&failure_detector_status);
// hack: set the servos in the failed bitmask to 0
if (status_updated) {
if (failure_detector_status.servo_to_center_mask) {
if (!_handled_servo_center_mask) {
PX4_WARN("Servo(s) to center failure injected");
_handled_servo_center_mask = failure_detector_status.servo_to_center_mask;
activation_updated = true;
}
} else {
if (_handled_servo_center_mask) {
PX4_INFO("Restoring servo(s)");
_handled_servo_center_mask = 0;
activation_updated = true;
}
}
}
if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
&& status_updated) {
if (failure_detector_status.fd_motor) {