From 0b274cc6e8e93b709ea0c50ef206747922e7d26f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 14 May 2015 00:54:41 +0200 Subject: [PATCH 1/2] PID1 Horizon Improved --- src/main/flight/pid.c | 34 ++++++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 74ad06dc2a..ec77c4bb16 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -659,6 +659,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 + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else if (pidProfile->D8[PIDLEVEL] >= 125){ + horizonLevelStrength = 100; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 100) * (125 / pidProfile->D8[PIDLEVEL])) + 100, 0, 100); + } + } + // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { uint8_t rate = controlRateConfig->rates[axis]; @@ -685,8 +715,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; From 3c1b678a37e9454e99fcf489820b63f7de49d181 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 14 May 2015 20:00:35 +0200 Subject: [PATCH 2/2] Fix truncation in horizonstrength calculation --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ec77c4bb16..bcb50cbb99 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -679,13 +679,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat 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 + // 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 = constrainf(((horizonLevelStrength - 100) * (125 / pidProfile->D8[PIDLEVEL])) + 100, 0, 100); + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 125) / 100) + 100, 0, 100); } }