Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 02:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

removed Rate_I from Loiter, lowing Rate_P from Loiter too. Too aggressive leads to rapid oscillations in air, and not around loiter position.

Browse Source
This commit is contained in:
Jason Short 2011-12-29 17:32:08 -08:00
parent 2f657ebdbf
commit 4bfa5def76
1 changed files with 25 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
ArduCopter/navigation.pde
View File

@ -44,6 +44,16 @@ static void calc_XY_velocity(){
// y_GPS_speed positve = Up // y_GPS_speed positve = Up
// x_GPS_speed positve = Right // x_GPS_speed positve = Right
// this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0/dTnav;
//int8_t tmp = 5;
int16_t x_diff = (g_gps->longitude - last_longitude) * tmp;
int16_t y_diff = (g_gps->latitude - last_latutude) * tmp;
// filter
x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
if(g_gps->ground_speed > 150){ if(g_gps->ground_speed > 150){
// Derive X/Y speed from GPS // Derive X/Y speed from GPS
// this is far more accurate when traveling about 1.5m/s // this is far more accurate when traveling about 1.5m/s
@ -51,16 +61,6 @@ static void calc_XY_velocity(){
x_GPS_speed = sin(temp) * (float)g_gps->ground_speed; x_GPS_speed = sin(temp) * (float)g_gps->ground_speed;
y_GPS_speed = cos(temp) * (float)g_gps->ground_speed; y_GPS_speed = cos(temp) * (float)g_gps->ground_speed;
}else{
// this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0/dTnav;
//int8_t tmp = 5;
int16_t x_diff = (g_gps->longitude - last_longitude) * tmp;
int16_t y_diff = (g_gps->latitude - last_latutude) * tmp;
// filter
x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
} }
last_longitude = g_gps->longitude; last_longitude = g_gps->longitude;
@ -78,7 +78,6 @@ static void calc_location_error(struct Location *next_loc)
1800 = 19.80m = 60 feet 1800 = 19.80m = 60 feet
3000 = 33m 3000 = 33m
10000 = 111m 10000 = 111m
pitch_max = 22° (2200)
*/ */
// X Error // X Error
@ -91,25 +90,24 @@ static void calc_location_error(struct Location *next_loc)
#define NAV_ERR_MAX 800 #define NAV_ERR_MAX 800
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);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
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
nav_lat = g.pi_nav_lat.get_p(y_rate_error);
nav_lat = constrain(nav_lat, -3500, 3500);
nav_lat += y_iterm;
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -1000, 1000);
y_rate_error = y_target_speed - y_actual_speed; // 413 nav_lon = g.pi_nav_lon.get_p(x_rate_error);
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum nav_lon = constrain(nav_lon, -3500, 3500);
nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); nav_lon += x_iterm;
nav_lat = constrain(nav_lat, -3500, 3500);
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 += x_iterm;
/* /*
int8_t ttt = 1.0/dTnav; int8_t ttt = 1.0/dTnav;