zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ACM: Disabled D term experiment for Loiter until more testing

mission-4.1.18
Jason Short 13 years ago
parent
a52a14bf12
commit
7153b94ade
1 changed files with 8 additions and 8 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      ArduCopter/navigation.pde

16
ArduCopter/navigation.pde
Unescape View File

@ -109,12 +109,12 @@ static void calc_location_error(struct Location *next_loc)
tmp = (long_error - last_lon_error); tmp = (long_error - last_lon_error);
if(abs(abs(tmp) -last_lon_d) > 20){ if(abs(abs(tmp) -last_lon_d) > 20){
tmp = x_rate_d; tmp = x_rate_d;
} }/*
if(long_error > 0){ if(long_error > 0){
if(tmp < 0) tmp = 0; if(tmp < 0) tmp = 0;
}else{ }else{
if(tmp > 0) tmp = 0; if(tmp > 0) tmp = 0;
} }*/
x_rate_d = lon_rate_d_filter.apply(tmp); x_rate_d = lon_rate_d_filter.apply(tmp);
x_rate_d = constrain(x_rate_d, -800, 800); x_rate_d = constrain(x_rate_d, -800, 800);
last_lon_d = abs(tmp); last_lon_d = abs(tmp);
@ -123,11 +123,11 @@ static void calc_location_error(struct Location *next_loc)
tmp = (lat_error - last_lat_error); tmp = (lat_error - last_lat_error);
if(abs(abs(tmp) -last_lat_d) > 20) if(abs(abs(tmp) -last_lat_d) > 20)
tmp = y_rate_d; tmp = y_rate_d;
if(lat_error > 0){ /*if(lat_error > 0){
if(tmp < 0) tmp = 0; if(tmp < 0) tmp = 0;
}else{ }else{
if(tmp > 0) tmp = 0; if(tmp > 0) tmp = 0;
} }*/
y_rate_d = lat_rate_d_filter.apply(tmp); y_rate_d = lat_rate_d_filter.apply(tmp);
y_rate_d = constrain(y_rate_d, -800, 800); y_rate_d = constrain(y_rate_d, -800, 800);
last_lat_d = abs(tmp); 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 = 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_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 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 = 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 += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
nav_lon = constrain(nav_lon, -3000, 3000); // 30° nav_lon = constrain(nav_lon, -3000, 3000); // 30°
// North / South // North / South
y_target_speed = g.pi_loiter_lat.get_p(y_error); y_target_speed = g.pi_loiter_lat.get_p(y_error);
//y_target_speed = constrain(y_target_speed, -250, 250); //y_target_speed = constrain(y_target_speed, -250, 250);
y_rate_error = y_target_speed - y_actual_speed; y_rate_error = y_target_speed - y_actual_speed;
nav_lat = g.pid_loiter_rate_lat.get_pi(y_rate_error, 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 += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav);
nav_lat = constrain(nav_lat, -3000, 3000); // 30° nav_lat = constrain(nav_lat, -3000, 3000); // 30°
// copy over I term to Nav_Rate // copy over I term to Nav_Rate

Write Preview
Loading…
Cancel
Save