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

HelicopterCoaxial: handle yaw saturation

Browse Source 
This had to be done for the integrators to work at all.
This commit is contained in:
Matthias Grob
2023-09-28 08:55:06 +02:00
committed by Beat Küng
parent cf40d95ef0
commit 20f2df4410
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp
+6 -6
View File
@@ -116,13 +116,13 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector
actuator_sp(0) = throttle - yaw; // Clockwise actuator_sp(0) = throttle - yaw; // Clockwise
actuator_sp(1) = throttle + yaw; // Counter-clockwise actuator_sp(1) = throttle + yaw; // Counter-clockwise
// Saturation check for yaw TODO check saturation // Saturation check for yaw
// if (actuator_sp(1) < actuator_min(1)) { if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) {
// setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
// } else if (actuator_sp(1) > actuator_max(1)) { } else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) {
// setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
// } }
for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;