Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

ACM: Disabled D term experiment for Loiter until more testing

Browse Source
This commit is contained in:
Jason Short 2012-03-15 19:20:03 -07:00
parent a52a14bf12
commit 7153b94ade
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/navigation.pde
View File

@ -109,12 +109,12 @@ static void calc_location_error(struct Location *next_loc)
tmp = (long_error - last_lon_error);
if(abs(abs(tmp) -last_lon_d) > 20){
tmp = x_rate_d;
}
}/*
if(long_error > 0){
if(tmp < 0) tmp = 0;
}else{
if(tmp > 0) tmp = 0;
}
}*/
x_rate_d = lon_rate_d_filter.apply(tmp);
x_rate_d = constrain(x_rate_d, -800, 800);
last_lon_d = abs(tmp);
@ -123,11 +123,11 @@ static void calc_location_error(struct Location *next_loc)
tmp = (lat_error - last_lat_error);
if(abs(abs(tmp) -last_lat_d) > 20)
tmp = y_rate_d;
if(lat_error > 0){
/*if(lat_error > 0){
if(tmp < 0) tmp = 0;
}else{
if(tmp > 0) tmp = 0;
}
}*/
y_rate_d = lat_rate_d_filter.apply(tmp);
y_rate_d = constrain(y_rate_d, -800, 800);
last_lat_d = abs(tmp);
@ -151,16 +151,16 @@ static void calc_loiter(int x_error, int y_error)
x_target_speed = g.pi_loiter_lon.get_p(x_error); // not contstrained yet
//x_target_speed = constrain(x_target_speed, -250, 250); // limit to 2.5m/s travel speed
x_rate_error = x_target_speed - x_actual_speed; // calc the speed error
nav_lon = g.pid_loiter_rate_lon.get_pi(x_rate_error, dTnav);
nav_lon += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
//nav_lon += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
nav_lon = constrain(nav_lon, -3000, 3000); // 30°
// North / South
y_target_speed = g.pi_loiter_lat.get_p(y_error);
//y_target_speed = constrain(y_target_speed, -250, 250);
y_rate_error = y_target_speed - y_actual_speed;
nav_lat = g.pid_loiter_rate_lat.get_pi(y_rate_error, dTnav);
nav_lat += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav);
nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav);
//nav_lat += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav);
nav_lat = constrain(nav_lat, -3000, 3000); // 30°
// copy over I term to Nav_Rate