From 3c1b678a37e9454e99fcf489820b63f7de49d181 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 14 May 2015 20:00:35 +0200 Subject: [PATCH] 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 ec77c4bb1..bcb50cbb9 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); } } -- 2.11.4.GIT