From c7da9c63f164dab8f6907bae1059de9a32f815bb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 13 Dec 2011 21:20:45 -0800 Subject: [PATCH] updated Loiter control to provide an alternative to GPS rate control for users with circling issues. Works in SIM, needs flight testing --- ArduCopter/APM_Config.h | 3 +++ ArduCopter/navigation.pde | 23 +++++++++++++++-------- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index f356b0ccc6..5cd0980765 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -65,3 +65,6 @@ // enable this for the new 'APM2' hardware // #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 + +#define LOITER_METHOD 0 +// set to 1 to try an alternative Loiter control method \ No newline at end of file diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index ed483f1774..3df800831e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -55,6 +55,8 @@ static void calc_location_error(struct Location *next_loc) } #define NAV_ERR_MAX 800 + +#if LOITER_METHOD == 0 static void calc_loiter(int x_error, int y_error) { x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); @@ -103,16 +105,20 @@ static void calc_loiter(int x_error, int y_error) nav_lon += x_iterm; } +#else static void calc_loiter2(int x_error, int y_error) { static int last_x_error = 0; static int last_y_error = 0; - x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //1200 y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); - int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); - int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); + int x_target_speed = g.pi_loiter_lon.get_p(x_error); + int y_target_speed = g.pi_loiter_lat.get_p(y_error); + + int x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); + int y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); // find the rates: x_actual_speed = (float)(last_x_error - x_error)/dTnav; @@ -122,16 +128,17 @@ static void calc_loiter2(int x_error, int y_error) last_x_error = x_error; last_y_error = y_error; - y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum + y_rate_error = y_target_speed - y_actual_speed; nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); - nav_lat = constrain(nav_lat, -3500, 3500); + nav_lat = constrain(nav_lat, -2200, 2200); + nav_lat += y_iterm; x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -1000, 1000); nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); - nav_lon = constrain(nav_lon, -3500, 3500); + nav_lon = constrain(nav_lon, -2200, 2200); + nav_lon += x_iterm; } +#endif // nav_roll, nav_pitch static void calc_loiter_pitch_roll()