Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +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]; ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported) //-----calculate D-term (Yaw D not yet supported)
if (axis == YAW) { 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 {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis]; delta = rD - lastRateError[axis];
lastRateError[axis] = rD; lastRateError[axis] = rD;
@ -317,6 +311,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); 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 // 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; ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term //-----calculate D-term
if (axis == YAW) { 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 (pidProfile->deltaMethod == DELTA_FROM_ERROR) { if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastRateError[axis]; delta = RateError - lastRateError[axis];
lastRateError[axis] = RateError; lastRateError[axis] = RateError;
@ -464,6 +451,19 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; 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; if (!pidStabilisationEnabled) axisPID[axis] = 0;

12
src/main/mw.c
View file

@ -815,6 +815,7 @@ void taskMainPidLoopCheck(void)
{ {
static uint32_t previousTime; static uint32_t previousTime;
static bool runTaskMainSubprocesses; static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
cycleTime = micros() - previousTime; cycleTime = micros() - previousTime;
previousTime = micros(); previousTime = micros();
@ -824,17 +825,6 @@ void taskMainPidLoopCheck(void)
debug[1] = averageSystemLoadPercent; 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) { if (runTaskMainSubprocesses) {
subTaskMainSubprocesses(); subTaskMainSubprocesses();
runTaskMainSubprocesses = false; runTaskMainSubprocesses = false;