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

ACM: Broke out the D term for the PID loop to add custom filtering. The current AC_PID filtering wasn't working for this application and we needed more smoothing. Bad pitch oscillations were being transmitted to the copter.

Browse Source
This commit is contained in:
Jason Short 2012-03-11 23:21:33 -07:00
parent 5c3cb1c5d0
commit 56d2b9ef05
1 changed files with 34 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduCopter/navigation.pde
View File

@ -55,11 +55,11 @@ static void calc_XY_velocity(){
// straightforward approach: // straightforward approach:
///* ///*
x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * scaleLongDown * tmp; x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * scaleLongDown * tmp;
y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp; y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp;
x_actual_speed = x_actual_speed >> 1; x_actual_speed = x_actual_speed >> 1;
y_actual_speed = y_actual_speed >> 1; y_actual_speed = y_actual_speed >> 1;
x_speed_old = x_actual_speed; x_speed_old = x_actual_speed;
y_speed_old = y_actual_speed; y_speed_old = y_actual_speed;
@ -82,6 +82,12 @@ static void calc_XY_velocity(){
static void calc_location_error(struct Location *next_loc) static void calc_location_error(struct Location *next_loc)
{ {
static int16_t last_lon_error = 0;
static int16_t last_lat_error = 0;
static int16_t last_lon_d = 0;
static int16_t last_lat_d = 0;
/* /*
Becuase we are using lat and lon to do our distance errors here's a quick chart: Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m 100 = 1m
@ -96,12 +102,32 @@ static void calc_location_error(struct Location *next_loc)
// Y Error // Y Error
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
int16_t tmp;
tmp = (long_error - last_lon_error);
if(abs(abs(tmp) -last_lon_d) > 15) tmp = x_rate_d;
x_rate_d = lon_rate_d_filter.apply(tmp);
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;
y_rate_d = lat_rate_d_filter.apply(tmp);
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);
last_lon_error = long_error;
last_lat_error = lat_error;
} }
#define NAV_ERR_MAX 600 #define NAV_ERR_MAX 600
static void calc_loiter(int x_error, int y_error) static void calc_loiter(int x_error, int y_error)
{ {
#if LOITER_RATE == 1
int16_t x_target_speed, y_target_speed; int16_t x_target_speed, y_target_speed;
//int16_t x_iterm, y_iterm; //int16_t x_iterm, y_iterm;
@ -109,31 +135,22 @@ 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_pid(x_rate_error, dTnav); 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 = 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_pid(y_rate_error, dTnav); 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 = 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
g.pid_nav_lon.set_integrator(g.pid_loiter_rate_lon.get_integrator()); g.pid_nav_lon.set_integrator(g.pid_loiter_rate_lon.get_integrator());
g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator()); g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator());
#else
// no rate control on Loiter
nav_lon = g.pid_loiter_rate_lon.get_pid(x_error, dTnav);
nav_lat = g.pid_loiter_rate_lat.get_pid(y_error, dTnav);
nav_lon = constrain(nav_lon, -3000, 3000); // 30°
nav_lat = constrain(nav_lat, -3000, 3000); // 30°
#endif
// Wind I term based on location error, // Wind I term based on location error,
// limit windup // limit windup