rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #891 from borisbstyle/pid1-horizon 

PID1 Horizon Mode Fixed
This commit is contained in:
Dominic Clifton 2015-05-14 19:31:45 +01:00
parent e69e999bdc 3c1b678a37
commit cf4a552b94
1 changed files with 32 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/main/flight/pid.c
View File

@ -660,6 +660,36 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
static int32_t lastError[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
int8_t horizonLevelStrength = 100;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
if(ABS(stickPosAil) > ABS(stickPosEle)){
mostDeflectedPos = ABS(stickPosAil);
}
else {
mostDeflectedPos = ABS(stickPosEle);
}
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection
// PID D Level Term is used for Horizon Sensitivity. It is adjusted so the default value of 100 works pretty well.
// Default Level D term of 100 equals to 80% sensitivity and 125 and above is 100% sensitivity. It is scaled to prevent too much truncating n integers
if(pidProfile->D8[PIDLEVEL] == 0){
horizonLevelStrength = 0;
} else if (pidProfile->D8[PIDLEVEL] >= 125){
horizonLevelStrength = 100;
} else {
horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 125) / 100) + 100, 0, 100);
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
@ -686,8 +716,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
// mix up angle error to desired AngleRateTmp to add a little auto-level feel. horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;