From 1e546b54be27fc607f1bd2f117952c190fd84d46 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 18 Aug 2012 00:02:30 -0700 Subject: [PATCH] ACM : Nav rate limit lowered to prevent bad oscillations due to GPS latency. --- ArduCopter/navigation.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index bb8b650670..acc888483d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -202,7 +202,7 @@ static void calc_nav_rate(int16_t max_speed) x_rate_error = x_target_speed - x_actual_speed; #endif - x_rate_error = constrain(x_rate_error, -1000, 1000); + x_rate_error = constrain(x_rate_error, -500, 500); nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav); int32_t tilt = (x_target_speed * x_target_speed * (int32_t)g.tilt_comp) / 10000; @@ -219,7 +219,7 @@ static void calc_nav_rate(int16_t max_speed) y_rate_error = y_target_speed - y_actual_speed; #endif - y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum + y_rate_error = constrain(y_rate_error, -500, 500); // added a rate error limit to keep pitching down to a minimum nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav); tilt = (y_target_speed * y_target_speed * (int32_t)g.tilt_comp) / 10000;