Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

lowered to nav_lat and nav_lon int16 

added a version that didn't have I term added to get a better indication of velocity estimation
This commit is contained in:
Jason Short 2011-12-29 21:35:01 -08:00
parent 89c705c8b9
commit 4535bc4fd9
3 changed files with 40 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/ArduCopter.pde
View File

@ -475,8 +475,12 @@ static int32_t nav_pitch; // deg * 100 : target pitch angle
static int32_t nav_yaw; // deg * 100 : target yaw angle static int32_t nav_yaw; // deg * 100 : target yaw angle
static int32_t home_to_copter_bearing; // deg * 100 : target yaw angle static int32_t home_to_copter_bearing; // deg * 100 : target yaw angle
static int32_t auto_yaw; // deg * 100 : target yaw angle static int32_t auto_yaw; // deg * 100 : target yaw angle
static int32_t nav_lat; // for error calcs
static int32_t nav_lon; // for error calcs static int16_t nav_lat; // for error calcs
static int16_t nav_lon; // for error calcs
static int16_t nav_lat_p; // for error calcs
static int16_t nav_lon_p; // for error calcs
static int16_t nav_throttle; // 0-1000 for throttle control static int16_t nav_throttle; // 0-1000 for throttle control
static int16_t crosstrack_error; static int16_t crosstrack_error;
@ -759,6 +763,7 @@ static void medium_loop()
y_GPS_speed = optflow.y_cm; y_GPS_speed = optflow.y_cm;
} }
#endif #endif
// control mode specific updates // control mode specific updates
// ----------------------------- // -----------------------------
update_navigation(); update_navigation();

4
ArduCopter/config.h
View File

@ -606,10 +606,10 @@
// Navigation control gains // Navigation control gains
// //
#ifndef LOITER_P #ifndef LOITER_P
# define LOITER_P .2 // # define LOITER_P .2 // .3 was too aggressive
#endif #endif
#ifndef LOITER_I #ifndef LOITER_I
# define LOITER_I 0.03 // Wind control # define LOITER_I 0.05 // Wind control
#endif #endif
#ifndef LOITER_IMAX #ifndef LOITER_IMAX
# define LOITER_IMAX 30 // degrees° # define LOITER_IMAX 30 // degrees°

61
ArduCopter/navigation.pde
View File

@ -47,6 +47,7 @@ static void calc_XY_velocity(){
// this speed is ~ in cm because we are using 10^7 numbers from GPS // this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0/dTnav; float tmp = 1.0/dTnav;
//int8_t tmp = 5; //int8_t tmp = 5;
int16_t x_diff = (g_gps->longitude - last_longitude) * tmp; int16_t x_diff = (g_gps->longitude - last_longitude) * tmp;
int16_t y_diff = (g_gps->latitude - last_latutude) * tmp; int16_t y_diff = (g_gps->latitude - last_latutude) * tmp;
@ -54,13 +55,12 @@ static void calc_XY_velocity(){
x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4; x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4; y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
if(g_gps->ground_speed > 150){ if(g_gps->ground_speed > 120){
// Derive X/Y speed from GPS // Derive X/Y speed from GPS
// this is far more accurate when traveling about 1.5m/s // this is far more accurate when traveling about 1.5m/s
float temp = g_gps->ground_course * RADX100; float temp = g_gps->ground_course * RADX100;
x_GPS_speed = sin(temp) * (float)g_gps->ground_speed; x_GPS_speed = sin(temp) * (float)g_gps->ground_speed;
y_GPS_speed = cos(temp) * (float)g_gps->ground_speed; y_GPS_speed = cos(temp) * (float)g_gps->ground_speed;
} }
last_longitude = g_gps->longitude; last_longitude = g_gps->longitude;
@ -90,41 +90,42 @@ static void calc_location_error(struct Location *next_loc)
#define NAV_ERR_MAX 800 #define NAV_ERR_MAX 800
static void calc_loiter(int x_error, int y_error) static void calc_loiter(int x_error, int y_error)
{ {
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); // East/West
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_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
nav_lat = g.pi_nav_lat.get_p(y_rate_error);
nav_lat = constrain(nav_lat, -3500, 3500);
nav_lat += y_iterm;
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);
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;
x_rate_error = constrain(x_rate_error, -1000, 1000); x_rate_error = constrain(x_rate_error, -1000, 1000);
nav_lon = g.pi_nav_lon.get_p(x_rate_error); nav_lon_p = g.pi_nav_lon.get_p(x_rate_error);
nav_lon = constrain(nav_lon, -3500, 3500); nav_lon_p = constrain(nav_lon_p, -3500, 3500);
nav_lon += x_iterm; nav_lon = nav_lon_p + x_iterm;
/* // 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_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_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
nav_lat_p = g.pi_nav_lat.get_p(y_rate_error);
nav_lat_p = constrain(nav_lat_p, -3500, 3500);
nav_lat = nav_lat_p + y_iterm;
///*
int8_t ttt = 1.0/dTnav; int8_t ttt = 1.0/dTnav;
int16_t t2 = g.pi_nav_lat.get_integrator(); int16_t t2 = g.pi_nav_lat.get_integrator();
// 1 2 3 4 5 6 7 8 9 10 11
Serial.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n", // 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 wp_distance, //1
ttt, //2 y_error, //2
y_error, //3 y_GPS_speed, //3
y_GPS_speed, //4 y_actual_speed, //4 ;
y_GPS_speed2, //5 y_target_speed, //5
y_actual_speed, //6 y_rate_error, //6
y_target_speed, //7 nav_lat_p, //7
y_rate_error, //8 nav_lat, //8
nav_lat, //9 y_iterm, //9
y_iterm, //10 t2); //10
t2); //11
//*/ //*/
/* /*
@ -143,7 +144,7 @@ static void calc_loiter(int x_error, int y_error)
//*/ //*/
} }
//wp_distance, y_error, y_GPS_speed, y_GPS_speed2, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2 //wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2
#define ERR_GAIN .01 #define ERR_GAIN .01