From a9610a0761340efb84e9603af6563cec8ce6f0e1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 5 Jun 2012 16:41:31 -0700 Subject: [PATCH] Stabilization patches MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit removed Angle error limit for stabilization constricted Iterm to +- 5° error and limited the implementation to when the quad is +- 5° from center doubled the output limit for Rate controller. increased default Rate_P gain to .18 with matching Rate_D of .004 Tested in the SIM and in backyard. dramatically increases performance and quad no longer overshoots and flips when pushed hard. --- ArduCopter/Attitude.pde | 45 ++++++++++++++++++++++++++--------------- ArduCopter/config.h | 10 ++++----- 2 files changed, 34 insertions(+), 21 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index d51b7c8965..245de6fc94 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -18,14 +18,18 @@ get_stabilize_roll(int32_t target_angle) return constrain(target_angle, -4500, 4500); #else - // limit the error we're feeding to the PID - target_angle = constrain(target_angle, -2500, 2500); - // convert to desired Rate: int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle); - int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt); - return get_rate_roll(target_rate) + iterm; + int16_t i_stab; + if(abs(ahrs.roll_sensor) < 500){ + target_angle = constrain(target_angle, -500, 500); + i_stab = g.pi_stabilize_roll.get_i(target_angle, G_Dt); + }else{ + i_stab = g.pi_stabilize_roll.get_integrator(); + } + + return get_rate_roll(target_rate) + i_stab; #endif } @@ -45,14 +49,19 @@ get_stabilize_pitch(int32_t target_angle) // output control: return constrain(target_angle, -4500, 4500); #else - // limit the error we're feeding to the PID - target_angle = constrain(target_angle, -2500, 2500); - // conver to desired Rate: + // convert to desired Rate: int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle); - int16_t iterm = g.pi_stabilize_pitch.get_i(target_angle, G_Dt); - return get_rate_pitch(target_rate) + iterm; + int16_t i_stab; + if(abs(ahrs.roll_sensor) < 500){ + target_angle = constrain(target_angle, -500, 500); + i_stab = g.pi_stabilize_pitch.get_i(target_angle, G_Dt); + }else{ + i_stab = g.pi_stabilize_pitch.get_integrator(); + } + return get_rate_pitch(target_rate) + i_stab; + #endif } @@ -158,7 +167,7 @@ get_rate_roll(int32_t target_rate) output -= rate_d_dampener; // constrain output - output = constrain(output, -2500, 2500); + output = constrain(output, -5000, 5000); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash @@ -209,7 +218,7 @@ get_rate_pitch(int32_t target_rate) output -= rate_d_dampener; // constrain output - output = constrain(output, -2500, 2500); + output = constrain(output, -5000, 5000); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash @@ -412,11 +421,15 @@ get_nav_yaw_offset(int yaw_input, int reset) static int16_t get_angle_boost(int16_t value) { - float temp = cos_pitch_x * cos_roll_x; - temp = 1.0 - constrain(temp, .5, 1.0); - int16_t output = temp * value; - return constrain(output, 0, 200); +// float temp = cos_pitch_x * cos_roll_x; +// temp = 1.0 - constrain(temp, .5, 1.0); +// int16_t output = temp * value; +// return constrain(output, 0, 200); // return (int)(temp * value); + + float temp = cos_pitch_x * cos_roll_x; + temp = constrain(temp, .5, 1.0); + return ((float)g.throttle_cruise / temp) - g.throttle_cruise; } #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3c64470acb..077b239f1b 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -654,26 +654,26 @@ // Stabilize Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P 0.14 +# define RATE_ROLL_P 0.18 #endif #ifndef RATE_ROLL_I # define RATE_ROLL_I 0.0 #endif #ifndef RATE_ROLL_D -# define RATE_ROLL_D 0.000 //.002 +# define RATE_ROLL_D 0.004 #endif #ifndef RATE_ROLL_IMAX # define RATE_ROLL_IMAX 5 // degrees #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.14 +# define RATE_PITCH_P 0.18 #endif #ifndef RATE_PITCH_I # define RATE_PITCH_I 0.0 #endif #ifndef RATE_PITCH_D -# define RATE_PITCH_D 0.00 +# define RATE_PITCH_D 0.004 #endif #ifndef RATE_PITCH_IMAX # define RATE_PITCH_IMAX 5 // degrees @@ -694,7 +694,7 @@ #ifndef STABILIZE_D -# define STABILIZE_D 0.15 +# define STABILIZE_D 0.00 #endif #ifndef STABILIZE_D_SCHEDULE