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

Enable Faster cycletimes (Sample Rates) on all targets // More automatic looptime calculations

Browse source 
cleanup
This commit is contained in:
borisbstyle 2016-02-24 16:36:12 +01:00
parent 981bddf182
commit f5de06c59e
12 changed files with 72 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,8 +49,8 @@
#include "config/runtime_config.h"
extern uint8_t motorCount;
extern float dT;
extern bool motorLimitReached;
uint32_t targetPidLooptime;
int16_t axisPID[3];
@ -77,6 +77,10 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
}
void pidResetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
@ -108,7 +112,7 @@ void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static float antiWindUpIncrement = 0;
if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetLooptime; // Calculate increment for 500ms period
if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetPidLooptime; // Calculate increment for 500ms period
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
@ -145,10 +149,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
int axis, deltaCount;
float horizonLevelStrength = 1;
float dT = (float)targetLooptime * 0.000001f;
float dT = (float)targetPidLooptime * 0.000001f;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
@ -284,7 +288,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
static int32_t previousAverageDelta[2];
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
@ -302,7 +306,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
gyroError = gyroADC[axis] >> 2;
error = rc - gyroError;
errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here
errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here
if (ABS(gyroADC[axis]) > (640 * 4)) {
errorGyroI[axis] = 0;
@ -439,7 +443,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
@ -502,7 +506,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * pidProfile->I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -530,7 +534,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta