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
|
#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)
|
||||||
{
|
{
|
||||||
|
int16_t x_target_speed, y_target_speed;
|
||||||
|
int16_t x_iterm, y_iterm;
|
||||||
|
|
||||||
// East / West
|
// East / West
|
||||||
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
|
x_target_speed = g.pi_loiter_lon.get_p(x_error); // not contstrained yet
|
||||||
x_target_speed = constrain(x_error, -250, 250);
|
x_target_speed = constrain(x_target_speed, -250, 250); // limit to 2.5m/s travel speed
|
||||||
// limit windup
|
x_rate_error = x_target_speed - x_actual_speed; // calc the speed error
|
||||||
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
|
||||||
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
|
nav_lon = constrain(nav_lon, -3000, 3000); // 30°
|
||||||
x_rate_error = x_target_speed - x_actual_speed;
|
|
||||||
|
|
||||||
// North / South
|
// North / South
|
||||||
int16_t 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_error, -250, 250);
|
y_target_speed = constrain(y_target_speed, -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);
|
|
||||||
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 = constrain(nav_lat, -3000, 3000); // 30°
|
||||||
|
|
||||||
//nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
|
// copy over I term to Nav_Rate
|
||||||
//nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav);
|
g.pid_nav_lon.set_integrator(g.pid_loiter_rate_lon.get_integrator());
|
||||||
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
|
g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator());
|
||||||
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);
|
|
||||||
|
|
||||||
|
// 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_lat = nav_lat + y_iterm;
|
||||||
nav_lon = nav_lon + x_iterm;
|
nav_lon = nav_lon + x_iterm;
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
int8_t ttt = 1.0/dTnav;
|
int8_t ttt = 1.0/dTnav;
|
||||||
|
@ -172,22 +176,29 @@ static void calc_nav_rate(int max_speed)
|
||||||
// nav_bearing includes crosstrack
|
// nav_bearing includes crosstrack
|
||||||
float temp = (9000l - nav_bearing) * RADX100;
|
float temp = (9000l - nav_bearing) * RADX100;
|
||||||
|
|
||||||
|
// East / West
|
||||||
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
|
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
|
||||||
x_rate_error = constrain(x_rate_error, -1000, 1000);
|
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 = (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
|
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_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_lat = constrain(nav_lat, -3000, 3000);
|
||||||
|
|
||||||
nav_lon = nav_lon + x_iterm;
|
// copy over I term to Loiter_Rate
|
||||||
nav_lat = nav_lat + y_iterm;
|
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",
|
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