fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-04-04 17:53:50 +02:00 committed by Beat Küng
parent cf40d95ef0
commit 20f2df4410
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp
View File

@ -116,13 +116,13 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector
actuator_sp(0) = throttle - yaw; // Clockwise
actuator_sp(1) = throttle + yaw; // Counter-clockwise
// Saturation check for yaw TODO check saturation
// if (actuator_sp(1) < actuator_min(1)) {
// setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
// Saturation check for yaw
if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) {
setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
// } else if (actuator_sp(1) > actuator_max(1)) {
// setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
// }
} else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) {
setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
}
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;