From 9190f1ad6be47d9da6bb78d558e842cff2cce6d7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 13 Nov 2015 15:17:43 +0100 Subject: [PATCH] Configurable Angle Sensitivity Luxfloat --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 372caba9d..bd27eb893 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -142,11 +142,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } else { // calculate error and limit the angle to the max inclination #ifdef GPS - errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; + errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else - errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; + errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) {