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

8
ArduCopter/config.h
View File

@ -467,7 +467,7 @@
// Navigation control gains // Navigation control gains
// //
#ifndef LOITER_P #ifndef LOITER_P
# define LOITER_P .4 // # define LOITER_P 1.0 //
#endif #endif
#ifndef LOITER_I #ifndef LOITER_I
# define LOITER_I 0.01 // # define LOITER_I 0.01 //
@ -480,14 +480,14 @@
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0.15 // this # define NAV_I 0.10 // this
#endif #endif
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 20 // degrees # define NAV_IMAX 16 // degrees
#endif #endif
#ifndef WAYPOINT_SPEED_MAX #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 #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 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 #define NAV_ERR_MAX 400
static void calc_loiter(int x_error, int y_error) 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); x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_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 #endif
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, -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); 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 = 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_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) 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() static long get_altitude_error()
{ {
return next_WP.alt - current_loc.alt; return next_WP.alt - current_loc.alt;