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

ACM: Loiter D performance updates

Browse Source
This commit is contained in:
Jason Short 2012-03-13 10:21:56 -07:00
parent 6edad4d9ef
commit 0edc1020ac
1 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduCopter/navigation.pde
View File

@ -105,21 +105,37 @@ static void calc_location_error(struct Location *next_loc)
int16_t tmp;
tmp = (long_error - last_lon_error);
if(abs(abs(tmp) -last_lon_d) > 15) tmp = x_rate_d;
// -------------------------------------
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);
tmp = (lat_error - last_lat_error);
if(abs(abs(tmp) -last_lat_d) > 15) tmp = y_rate_d;
//if(abs(tmp) > 80) tmp = y_rate_d;
// -------------------------------------
tmp = (lat_error - last_lat_error);
if(abs(abs(tmp) -last_lat_d) > 20)
tmp = y_rate_d;
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);
int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
int16_t raww = (long_error - last_lon_error);
//Serial.printf("XX, %d, %d, %d\n", raww, x_rate_d, t22);
// debug
//int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
//if(control_mode == LOITER)
// Serial.printf("XX, %d, %d, %d \n", long_error, t22, (int16_t)g.pid_loiter_rate_lon.get_integrator());
last_lon_error = long_error;
last_lat_error = lat_error;