turned off Stabilize_I by default because it was hurting loiter

tweaked speed control of rate_nav
This commit is contained in:
Jason Short 2011-09-24 21:49:45 -07:00
parent b6b2606442
commit a2886fc991
2 changed files with 13 additions and 9 deletions

View File

@ -406,20 +406,20 @@
# define STABILIZE_ROLL_P 4.2 # define STABILIZE_ROLL_P 4.2
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.008 # define STABILIZE_ROLL_I 0.001
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 3 // degrees # define STABILIZE_ROLL_IMAX 0 // degrees
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.2 # define STABILIZE_PITCH_P 4.2
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.008 # define STABILIZE_PITCH_I 0.001
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 3 // degrees # define STABILIZE_PITCH_IMAX 0 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -502,7 +502,7 @@
#endif #endif
#ifndef WAYPOINT_SPEED_MAX #ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph # define WAYPOINT_SPEED_MAX 400 // for 6m/s error = 13mph
#endif #endif

View File

@ -116,12 +116,16 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
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);
} }
#define NAV2_ERR_MAX 600.0
static void calc_nav_rate2(int max_speed) static void calc_nav_rate2(int max_speed)
{ {
float scaler = (float)wp_distance / NAV2_ERR_MAX; /*
scaler = constrain(scaler, 0.0, 1.0); 0 1 2 3 4 5 6 7 8
max_speed = (float)max_speed * scaler; ...|...|...|...|...|...|...|...|
100 200 300 400
+|+
*/
max_speed = min(max_speed, (wp_distance * 50));
// XXX target_angle should be the original desired target angle! // XXX target_angle should be the original desired target angle!
float temp = radians((original_target_bearing - g_gps->ground_course)/100.0); float temp = radians((original_target_bearing - g_gps->ground_course)/100.0);