Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

PID: Fix for I-term windup in PASSTHRU more (airplanes)

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-03-16 10:03:31 +10:00
parent 4c6de77ae8
commit a073f3ce11
1 changed files with 28 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

50
src/main/mw.c
View file

@ -363,28 +363,6 @@ void processRx(void)
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
if (throttleStatus == THROTTLE_LOW) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
if (rollPitchStatus == CENTERED) {
ENABLE_STATE(ANTI_WINDUP);
}
else {
DISABLE_STATE(ANTI_WINDUP);
}
}
else {
pidResetErrorGyro();
}
}
else {
DISABLE_STATE(ANTI_WINDUP);
}
// When armed and motors aren't spinning, do beeps and then disarm // When armed and motors aren't spinning, do beeps and then disarm
// board after delay so users without buzzer won't lose fingers. // board after delay so users without buzzer won't lose fingers.
// mixTable constrains motor commands, so checking throttleStatus is enough // mixTable constrains motor commands, so checking throttleStatus is enough
@ -498,6 +476,34 @@ void processRx(void)
DISABLE_FLIGHT_MODE(PASSTHRU_MODE); DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
} }
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
if (FLIGHT_MODE(PASSTHRU_MODE)) {
/* In PASSTHRU mode anti-windup must be explicitly enabled to prevent I-term wind-up (PID output is not used in PASSTHRU) */
ENABLE_STATE(ANTI_WINDUP);
}
else {
if (throttleStatus == THROTTLE_LOW) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
if (rollPitchStatus == CENTERED) {
ENABLE_STATE(ANTI_WINDUP);
}
else {
DISABLE_STATE(ANTI_WINDUP);
}
}
else {
pidResetErrorGyro();
}
}
else {
DISABLE_STATE(ANTI_WINDUP);
}
}
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE); DISABLE_FLIGHT_MODE(HEADFREE_MODE);
} }