mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Fix some BlackBox isues, remove redundant code from PID controllers.
This commit is contained in:
parent
e7e297ad53
commit
fe2f2f3053
4 changed files with 6 additions and 20 deletions
|
@ -456,11 +456,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
|
||||
axisPID[FD_YAW] = PTerm + ITerm;
|
||||
|
||||
if (motorCount >= 4) { // prevent "yaw jump" during yaw correction
|
||||
int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
|
||||
axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(FD_YAW);
|
||||
|
@ -594,11 +589,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
|
|||
|
||||
axisPID[FD_YAW] = PTerm + ITerm;
|
||||
|
||||
if (motorCount >= 4) { // prevent "yaw jump" during yaw correction
|
||||
int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
|
||||
axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(FD_YAW);
|
||||
|
@ -749,11 +739,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
|||
axisPID[FD_YAW] = PTerm + ITerm;
|
||||
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result.
|
||||
|
||||
if (motorCount >= 4) { // prevent "yaw jump" during yaw correction
|
||||
int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
|
||||
axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(FD_YAW);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue