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

removed Xtrack and increased rate error limit

This commit is contained in:
Jason Short 2011-09-11 20:36:20 -07:00
parent 285029856b
commit bae05178f0
1 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/navigation.pde
View File

@ -34,11 +34,12 @@ static void navigate()
// nav_bearing will include xtrac correction // nav_bearing will include xtrac correction
// ----------------------------------------- // -----------------------------------------
//xtrack_enabled = false; //xtrack_enabled = false;
if(xtrack_enabled){
nav_bearing = wrap_360(target_bearing + get_crosstrack_correction()); //if(xtrack_enabled){
}else{ // crosstrack_correction = get_crosstrack_correction();
nav_bearing = target_bearing; //}else {
} // crosstrack_correction = 0;
//}
} }
static bool check_missed_wp() static bool check_missed_wp()
@ -101,18 +102,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
} }
// find the rates: // find the rates:
float temp = radians((float)g_gps->ground_course/100.0);
// calc the cos of the error to tell how fast we are moving towards the target in cm // calc the cos of the error to tell how fast we are moving towards the target in cm
y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); y_actual_speed = (float)g_gps->ground_speed * cos(temp);
y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
//Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
// calc the sin of the error to tell how fast we are moving laterally to the target in cm // calc the sin of the error to tell how fast we are moving laterally to the target in cm
x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); x_actual_speed = (float)g_gps->ground_speed * sin(temp);
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, -250, 250); x_rate_error = constrain(x_rate_error, -600, 600);
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
} }
@ -128,11 +131,11 @@ static void calc_nav_pitch_roll()
} }
// ------------------------------ // ------------------------------
static void calc_bearing_error() /*static void calc_bearing_error()
{ {
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }*/
static long get_altitude_error() static long get_altitude_error()
{ {
@ -189,6 +192,7 @@ static long wrap_180(long error)
return error; return error;
} }
/*
static long get_crosstrack_correction(void) static long get_crosstrack_correction(void)
{ {
// Crosstrack Error // Crosstrack Error
@ -206,7 +210,7 @@ static long get_crosstrack_correction(void)
} }
return 0; return 0;
} }
*/
static long cross_track_test() static long cross_track_test()
{ {