aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 7953808d14
commit 468d9c4b4f
1 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/navigation.pde
View File

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