Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
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.

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

26
ArduCopter/navigation.pde
View File

@ -44,14 +44,6 @@ static void calc_XY_velocity(){
// y_GPS_speed positve = Up
// x_GPS_speed positve = Right
if(g_gps->ground_speed > 150){
// Derive X/Y speed from GPS
// this is far more accurate when traveling about 1.5m/s
float temp = g_gps->ground_course * RADX100;
x_GPS_speed = sin(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;
@ -61,6 +53,14 @@ static void calc_XY_velocity(){
// 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){
// Derive X/Y speed from GPS
// this is far more accurate when traveling about 1.5m/s
float temp = g_gps->ground_course * RADX100;
x_GPS_speed = sin(temp) * (float)g_gps->ground_speed;
y_GPS_speed = cos(temp) * (float)g_gps->ground_speed;
}
last_longitude = g_gps->longitude;
@ -78,7 +78,6 @@ static void calc_location_error(struct Location *next_loc)
1800 = 19.80m = 60 feet
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
*/
// X Error
@ -94,20 +93,19 @@ static void calc_loiter(int x_error, int y_error)
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
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 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_pi(y_rate_error, dTnav);
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_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
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_p(x_rate_error);
nav_lon = constrain(nav_lon, -3500, 3500);
nav_lon += x_iterm;