From 25529dec696cb4ac0567cbf79a0e1a3deec3d082 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 22 Jan 2012 14:19:21 -0800 Subject: [PATCH] added loiter_d to allow users to configure alternate Loiter alg --- ArduCopter/navigation.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 02eabe763d..a8795de1eb 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -146,10 +146,10 @@ static void calc_location_error(struct Location *next_loc) static void calc_loiter1(int x_error, int y_error) { int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav); - int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps + int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav); - int16_t lat_D = 3 * y_actual_speed ; + int16_t lat_D = g.loiter_d * y_actual_speed ; //limit of terms lon_D = constrain(lon_D,-500,500);