mirror of https://github.com/ArduPilot/ardupilot
Loiter updates
This commit is contained in:
parent
58bca7d4a4
commit
22982cda0f
|
@ -101,33 +101,37 @@ static void calc_location_error(struct Location *next_loc)
|
|||
#define NAV_ERR_MAX 600
|
||||
static void calc_loiter(int x_error, int y_error)
|
||||
{
|
||||
// East/West
|
||||
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
|
||||
x_target_speed = constrain(x_error, -250, 250);
|
||||
// limit windup
|
||||
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
|
||||
x_rate_error = x_target_speed - x_actual_speed;
|
||||
int16_t x_target_speed, y_target_speed;
|
||||
int16_t x_iterm, y_iterm;
|
||||
|
||||
// North/South
|
||||
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
|
||||
y_target_speed = constrain(y_error, -250, 250);
|
||||
// limit windup
|
||||
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
|
||||
// East / West
|
||||
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_pid(x_rate_error, 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_pid(y_rate_error, dTnav);
|
||||
nav_lat = constrain(nav_lat, -3000, 3000); // 30°
|
||||
|
||||
//nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
|
||||
//nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav);
|
||||
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
|
||||
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
|
||||
nav_lon = constrain(nav_lon, -3000, 3000);
|
||||
nav_lat = constrain(nav_lat, -3000, 3000);
|
||||
// copy over I term to Nav_Rate
|
||||
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());
|
||||
|
||||
// Wind I term based on location error,
|
||||
// limit windup
|
||||
/*
|
||||
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
|
||||
y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
|
||||
nav_lat = nav_lat + y_iterm;
|
||||
nav_lon = nav_lon + x_iterm;
|
||||
|
||||
|
||||
*/
|
||||
|
||||
/*
|
||||
int8_t ttt = 1.0/dTnav;
|
||||
|
@ -172,22 +176,29 @@ static void calc_nav_rate(int max_speed)
|
|||
// nav_bearing includes crosstrack
|
||||
float temp = (9000l - nav_bearing) * RADX100;
|
||||
|
||||
// East / West
|
||||
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
|
||||
x_rate_error = constrain(x_rate_error, -1000, 1000);
|
||||
int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
|
||||
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
|
||||
nav_lon = constrain(nav_lon, -3000, 3000);
|
||||
|
||||
// North / South
|
||||
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413
|
||||
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
|
||||
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
|
||||
|
||||
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
|
||||
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
|
||||
|
||||
nav_lon = constrain(nav_lon, -3000, 3000);
|
||||
nav_lat = constrain(nav_lat, -3000, 3000);
|
||||
|
||||
nav_lon = nav_lon + x_iterm;
|
||||
nav_lat = nav_lat + y_iterm;
|
||||
// copy over I term to Loiter_Rate
|
||||
g.pid_loiter_rate_lon.set_integrator(g.pid_nav_lon.get_integrator());
|
||||
g.pid_loiter_rate_lat.set_integrator(g.pid_nav_lat.get_integrator());
|
||||
|
||||
//int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
|
||||
//int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
|
||||
|
||||
//nav_lon = nav_lon + x_iterm;
|
||||
//nav_lat = nav_lat + y_iterm;
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",
|
||||
|
|
Loading…
Reference in New Issue