mirror of https://github.com/ArduPilot/ardupilot
navigation.pde:
removed unneeded WP_Distance check remove unneeded return for Navigate removed unneeded rate_D calcs removed unused functions
This commit is contained in:
Jason Short
parent
7e190c6f5c
commit
9732b7d2dc
|
|
@ -3,18 +3,13 @@
|
|||
//****************************************************************
|
||||
// Function that will calculate the desired direction to fly and distance
|
||||
//****************************************************************
|
||||
static byte navigate()
|
||||
static void navigate()
|
||||
{
|
||||
// waypoint distance from plane in cm
|
||||
// ---------------------------------------
|
||||
wp_distance = get_distance(¤t_loc, &next_WP);
|
||||
home_distance = get_distance(¤t_loc, &home);
|
||||
|
||||
if (wp_distance < 0){
|
||||
// something went very wrong
|
||||
return 0;
|
||||
}
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
|
|
@ -23,8 +18,6 @@ static byte navigate()
|
|||
// nav_bearing will includes xtrac correction
|
||||
// ------------------------------------------
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
static bool check_missed_wp()
|
||||
|
|
@ -36,8 +29,8 @@ static bool check_missed_wp()
|
|||
}
|
||||
|
||||
// ------------------------------
|
||||
|
||||
static void calc_XY_velocity(){
|
||||
// called after GPS read
|
||||
// offset calculation of GPS speed:
|
||||
// used for estimations below 1.5m/s
|
||||
// our GPS is about 1m per
|
||||
|
|
@ -80,12 +73,6 @@ static void calc_XY_velocity(){
|
|||
|
||||
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:
|
||||
100 = 1m
|
||||
|
|
@ -100,43 +87,6 @@ static void calc_location_error(struct Location *next_loc)
|
|||
|
||||
// Y Error
|
||||
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) > 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) > 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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
#define NAV_ERR_MAX 600
|
||||
|
|
@ -212,54 +162,6 @@ static void calc_loiter(int x_error, int y_error)
|
|||
// 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());
|
||||
|
||||
//Serial.printf("XX, %d, %d, %d\n", long_error, x_actual_speed, (int16_t)g.pid_loiter_rate_lon.get_integrator());
|
||||
|
||||
// Wind I term based on location error,
|
||||
// limit windup
|
||||
/*
|
||||
int16_t x_iterm, y_iterm;
|
||||
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;
|
||||
int16_t t2 = g.pi_nav_lat.get_integrator();
|
||||
|
||||
// 1 2 3 4 5 6 7 8 9 10
|
||||
Serial.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
|
||||
wp_distance, //1
|
||||
y_error, //2
|
||||
y_GPS_speed, //3
|
||||
|
||||
y_actual_speed, //4 ;
|
||||
y_target_speed, //5
|
||||
y_rate_error, //6
|
||||
nav_lat_p, //7
|
||||
nav_lat, //8
|
||||
y_iterm, //9
|
||||
t2); //10
|
||||
//*/
|
||||
|
||||
/*
|
||||
int16_t t1 = g.pid_nav_lon.get_integrator(); // X
|
||||
Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n",
|
||||
wp_distance, //1
|
||||
dTnav, //2
|
||||
x_error, //3
|
||||
x_GPS_speed, //4
|
||||
x_actual_speed, //5
|
||||
x_target_speed, //6
|
||||
x_rate_error, //7
|
||||
nav_lat, //8
|
||||
x_iterm, //9
|
||||
t1); //10
|
||||
//*/
|
||||
}
|
||||
|
||||
static void calc_nav_rate(int max_speed)
|
||||
|
|
@ -286,77 +188,8 @@ static void calc_nav_rate(int max_speed)
|
|||
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",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat,
|
||||
x_iterm,
|
||||
y_iterm,
|
||||
crosstrack_error);
|
||||
//*/
|
||||
|
||||
// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
|
||||
|
||||
/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat);*/
|
||||
}
|
||||
|
||||
|
||||
/*static void calc_nav_lon(int rate)
|
||||
{
|
||||
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
|
||||
nav_lon = constrain(nav_lon, -3000, 3000);
|
||||
}
|
||||
|
||||
static void calc_nav_lat(int rate)
|
||||
{
|
||||
nav_lat = g.pid_nav_lat.get_pid(rate, dTnav);
|
||||
nav_lat = constrain(nav_lat, -3000, 3000);
|
||||
}
|
||||
*/
|
||||
|
||||
//static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
|
||||
/*{
|
||||
int16_t tt = desired_rate;
|
||||
// scale down the desired rate and square it
|
||||
desired_rate = desired_rate / 20;
|
||||
desired_rate = desired_rate * desired_rate;
|
||||
int16_t tmp = 0;
|
||||
|
||||
if (tt > 0){
|
||||
tmp = rate_out + (rate_out - desired_rate);
|
||||
tmp = max(tmp, rate_out);
|
||||
}else if (tt < 0){
|
||||
tmp = rate_out + (rate_out + desired_rate);
|
||||
tmp = min(tmp, rate_out);
|
||||
}
|
||||
//Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp);
|
||||
return tmp;
|
||||
}*/
|
||||
|
||||
//wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2
|
||||
|
||||
|
||||
|
||||
// this calculation rotates our World frame of reference to the copter's frame of reference
|
||||
// We use the DCM's matrix to precalculate these trig values at 50hz
|
||||
static void calc_loiter_pitch_roll()
|
||||
|
|
|
|||
Loading…
Reference in New Issue