From 7153b94ade3e403513b35b3f4400b01ae4a5554e Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 15 Mar 2012 19:20:03 -0700 Subject: [PATCH] ACM: Disabled D term experiment for Loiter until more testing --- ArduCopter/navigation.pde | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 565e642a7a..d913efa84f 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -109,12 +109,12 @@ static void calc_location_error(struct Location *next_loc) tmp = (long_error - last_lon_error); if(abs(abs(tmp) -last_lon_d) > 20){ tmp = x_rate_d; - } + }/* if(long_error > 0){ if(tmp < 0) tmp = 0; }else{ if(tmp > 0) tmp = 0; - } + }*/ x_rate_d = lon_rate_d_filter.apply(tmp); x_rate_d = constrain(x_rate_d, -800, 800); last_lon_d = abs(tmp); @@ -123,11 +123,11 @@ static void calc_location_error(struct Location *next_loc) tmp = (lat_error - last_lat_error); if(abs(abs(tmp) -last_lat_d) > 20) tmp = y_rate_d; - if(lat_error > 0){ + /*if(lat_error > 0){ if(tmp < 0) tmp = 0; }else{ if(tmp > 0) tmp = 0; - } + }*/ y_rate_d = lat_rate_d_filter.apply(tmp); y_rate_d = constrain(y_rate_d, -800, 800); last_lat_d = abs(tmp); @@ -151,16 +151,16 @@ static void calc_loiter(int x_error, int y_error) x_target_speed = g.pi_loiter_lon.get_p(x_error); // not contstrained yet //x_target_speed = constrain(x_target_speed, -250, 250); // limit to 2.5m/s travel speed x_rate_error = x_target_speed - x_actual_speed; // calc the speed error - nav_lon = g.pid_loiter_rate_lon.get_pi(x_rate_error, dTnav); - nav_lon += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); + nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav); + //nav_lon += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); nav_lon = constrain(nav_lon, -3000, 3000); // 30° // North / South y_target_speed = g.pi_loiter_lat.get_p(y_error); //y_target_speed = constrain(y_target_speed, -250, 250); y_rate_error = y_target_speed - y_actual_speed; - nav_lat = g.pid_loiter_rate_lat.get_pi(y_rate_error, dTnav); - nav_lat += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav); + nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav); + //nav_lat += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav); nav_lat = constrain(nav_lat, -3000, 3000); // 30° // copy over I term to Nav_Rate