aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

updated Loiter control to provide an alternative to GPS rate control for users with circling issues. Works in SIM, needs flight testing

Browse Source
This commit is contained in:
Jason Short 2011-12-13 21:20:45 -08:00
parent d5666f287c
commit c7da9c63f1
2 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/APM_Config.h
View File

@ -65,3 +65,6 @@
// enable this for the new 'APM2' hardware // enable this for the new 'APM2' hardware
// #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 // #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
#define LOITER_METHOD 0
// set to 1 to try an alternative Loiter control method

23
ArduCopter/navigation.pde
View File

@ -55,6 +55,8 @@ static void calc_location_error(struct Location *next_loc)
} }
#define NAV_ERR_MAX 800 #define NAV_ERR_MAX 800
#if LOITER_METHOD == 0
static void calc_loiter(int x_error, int y_error) static void calc_loiter(int x_error, int y_error)
{ {
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); 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; nav_lon += x_iterm;
} }
#else
static void calc_loiter2(int x_error, int y_error) static void calc_loiter2(int x_error, int y_error)
{ {
static int last_x_error = 0; static int last_x_error = 0;
static int last_y_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); 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 x_target_speed = g.pi_loiter_lon.get_p(x_error);
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); 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: // find the rates:
x_actual_speed = (float)(last_x_error - x_error)/dTnav; 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_x_error = x_error;
last_y_error = y_error; last_y_error = y_error;
y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = y_target_speed - y_actual_speed;
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); 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 = 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 = 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 // nav_roll, nav_pitch
static void calc_loiter_pitch_roll() static void calc_loiter_pitch_roll()