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

Attempting to mimic loiter values for 4.0.42 

lowered Rate Error max in loiter
This commit is contained in:
Jason Short 2011-09-28 23:11:19 -07:00
parent a790d1ef7a
commit b805263974
3 changed files with 23 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/ArduCopter.pde
View File

@ -666,6 +666,8 @@ static void medium_loop()
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
}
}else{
g_gps->new_data = false;
}
break;
@ -921,12 +923,13 @@ static void update_GPS(void)
//current_loc.lat = -1224318000; // Lat * 10 * *7
//current_loc.alt = 100; // alt * 10 * *7
//return;
if(gps_watchdog < 10){
if(gps_watchdog < 12){
gps_watchdog++;
}else{
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
nav_roll >>= 1;
nav_pitch >>= 1;
// commented temporarily
//nav_roll >>= 1;
//nav_pitch >>= 1;
}
if (g_gps->new_data && g_gps->fix) {

8
ArduCopter/config.h
View File

@ -467,7 +467,7 @@
// Navigation control gains
//
#ifndef LOITER_P
# define LOITER_P .4 //
# define LOITER_P 1.0 //
#endif
#ifndef LOITER_I
# define LOITER_I 0.01 //
@ -480,14 +480,14 @@
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
#endif
#ifndef NAV_I
# define NAV_I 0.15 // this
# define NAV_I 0.10 // this
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 20 // degrees
# define NAV_IMAX 16 // degrees
#endif
#ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 400 // for 6m/s error = 13mph
# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph
#endif

33
ArduCopter/navigation.pde
View File

@ -55,14 +55,9 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
}
// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0);
#define NAV_ERR_MAX 400
static void calc_loiter(int x_error, int y_error)
{
// moved to globals for logging
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
@ -87,16 +82,25 @@ static void calc_loiter(int x_error, int y_error)
#endif
y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum
y_rate_error = constrain(y_rate_error, -250, 250); // 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);
//Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -600, 600);
x_rate_error = constrain(x_rate_error, -250, 250);
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
}
// nav_roll, nav_pitch
static void calc_loiter_pitch_roll()
{
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;
}
static void calc_nav_rate(int max_speed)
{
/*
@ -153,17 +157,6 @@ static void calc_nav_pitch_roll()
}
// nav_roll, nav_pitch
static void calc_loiter_pitch_roll()
{
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;
}
static long get_altitude_error()
{
return next_WP.alt - current_loc.alt;