From 98b258e83f59d2f89f66526ffa74c15719e905d1 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 2 Jan 2015 19:20:30 +0000 Subject: [PATCH] Ensure that rcRate great than 1.0 doesn't cause stability issues when aircraft is inverted. See #281. --- src/main/flight/flight.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index f6dea18ce3..22ee35fc93 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -189,7 +189,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa UNUSED(controlRateConfig); // **** PITCH & ROLL & YAW PID **** - prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500] + prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500] + for (axis = 0; axis < 3; axis++) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC // observe max inclination