From b6b0e57b52894fa363104e4027d09a3032f6c4b2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 3 Sep 2016 02:20:20 +0200 Subject: [PATCH] Change rcInput to store actual stick input --- src/main/mw.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 01322d98ca..05f20cb918 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -189,19 +189,17 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); rcCommandf = rc / 500.0f; + rcInput[axis] = ABS(rcCommandf); if (rcExpo) { float expof = rcExpo / 100.0f; - float absRc = ABS(rcCommandf); - rcCommandf = rcCommandf * (expof * (powerf(absRc, currentControlRateProfile->rcExpoPwr)) + absRc*(1-expof)); + rcCommandf = rcCommandf * (expof * (powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr)) + rcInput[axis]*(1-expof)); } - rcInput[axis] = ABS(rcCommandf); - angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { - rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate *= rcSuperfactor; } @@ -210,7 +208,7 @@ float calculateSetpointRate(int axis, int16_t rc) { } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) }