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

Update src/modules/fw_att_control/FixedwingAttitudeControl.cpp

Browse Source 
reduce code duplication

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Silvan Fuhrer
2022-08-17 18:22:29 +02:00
parent 70134296ed
commit b87afb138c
1 changed files with 10 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+10 -32
View File
@@ -419,45 +419,23 @@ void FixedwingAttitudeControl::Run()
// update saturation status from control allocation feedback
control_allocator_status_s control_allocator_status;
if (_vehicle_status.is_vtol) {
if (_control_allocator_status_subs[1].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
} else {
if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
/* Prepare data for attitude controllers */