Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Merge branch 'master' of https://github.com/borisbstyle/betaflight into PikoBLX_target_bf

Browse source
This commit is contained in:
NightHawk32 2016-08-23 15:58:48 -04:00
commit cc0d3187f1
2 changed files with 22 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

42
src/main/flight/pid.c
View file

@ -286,13 +286,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported)
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
} else {
if (axis != YAW) {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
lastRateError[axis] = rD;
@ -317,6 +311,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900);
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
}
// Disable PID control at zero throttle
@ -422,20 +422,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
DTerm = 0; // needed for blackbox
} else {
if (axis != YAW) {
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastRateError[axis];
lastRateError[axis] = RateError;
@ -464,6 +451,19 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
DTerm = 0; // needed for blackbox
}
if (!pidStabilisationEnabled) axisPID[axis] = 0;

12
src/main/mw.c
View file

@ -815,6 +815,7 @@ void taskMainPidLoopCheck(void)
{
static uint32_t previousTime;
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
cycleTime = micros() - previousTime;
previousTime = micros();
@ -824,17 +825,6 @@ void taskMainPidLoopCheck(void)
debug[1] = averageSystemLoadPercent;
}
const uint32_t startTime = micros();
while (true) {
if (gyroSyncCheckUpdate(&gyro)) {
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
break;
}
}
static uint8_t pidUpdateCountdown;
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses();
runTaskMainSubprocesses = false;