From b87afb138caa4737db6578c2ce645501cb1ce36b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 17 Aug 2022 18:22:29 +0200 Subject: [PATCH] Update src/modules/fw_att_control/FixedwingAttitudeControl.cpp reduce code duplication Co-authored-by: Mathieu Bresciani --- .../FixedwingAttitudeControl.cpp | 42 +++++-------------- 1 file changed, 10 insertions(+), 32 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index eeddc403bc..b421238f7f 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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 saturation_positive; - Vector saturation_negative; + if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { + Vector saturation_positive; + Vector 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 saturation_positive; - Vector 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 */