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

tweaks to Loiter

Browse Source
This commit is contained in:
Jason Short 2012-01-30 20:58:19 -08:00
parent 06d54bf0e8
commit 5de459a160
1 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/navigation.pde
View File

@ -44,8 +44,8 @@ static void calc_XY_velocity(){
static int32_t last_longitude = 0; static int32_t last_longitude = 0;
static int32_t last_latitude = 0; static int32_t last_latitude = 0;
//static int16_t x_speed_old = 0; static int16_t x_speed_old = 0;
//static int16_t y_speed_old = 0; static int16_t y_speed_old = 0;
// y_GPS_speed positve = Up // y_GPS_speed positve = Up
// x_GPS_speed positve = Right // x_GPS_speed positve = Right
@ -55,8 +55,14 @@ static void calc_XY_velocity(){
// straightforward approach: // straightforward approach:
///* ///*
x_actual_speed = (float)(g_gps->longitude - last_longitude) * tmp; x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * tmp;
y_actual_speed = (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;
y_actual_speed = y_actual_speed >> 1;
x_speed_old = x_actual_speed;
y_speed_old = y_actual_speed;
/* /*
// Ryan Beall's forward estimator: // Ryan Beall's forward estimator:
@ -92,18 +98,22 @@ static void calc_location_error(struct Location *next_loc)
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
} }
#define NAV_ERR_MAX 800 #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)
{ {
// East/West // East/West
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
x_target_speed = constrain(x_error, -150, 150);
// 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); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
x_rate_error = x_target_speed - x_actual_speed; x_rate_error = x_target_speed - x_actual_speed;
// North/South // North/South
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error); int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
y_target_speed = constrain(y_error, -150, 150);
// 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); 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;