aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Stabilization patches

Browse Source 
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.
This commit is contained in:
Jason Short 2012-06-05 16:41:31 -07:00
parent 70b04d9427
commit a9610a0761
2 changed files with 34 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduCopter/Attitude.pde
View File

@ -18,14 +18,18 @@ get_stabilize_roll(int32_t target_angle)
return constrain(target_angle, -4500, 4500); return constrain(target_angle, -4500, 4500);
#else #else
// limit the error we're feeding to the PID
target_angle = constrain(target_angle, -2500, 2500);
// convert to desired Rate: // convert to desired Rate:
int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle); 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 #endif
} }
@ -45,14 +49,19 @@ get_stabilize_pitch(int32_t target_angle)
// output control: // output control:
return constrain(target_angle, -4500, 4500); return constrain(target_angle, -4500, 4500);
#else #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); 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 #endif
} }
@ -158,7 +167,7 @@ get_rate_roll(int32_t target_rate)
output -= rate_d_dampener; output -= rate_d_dampener;
// constrain output // constrain output
output = constrain(output, -2500, 2500); output = constrain(output, -5000, 5000);
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash 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; output -= rate_d_dampener;
// constrain output // constrain output
output = constrain(output, -2500, 2500); output = constrain(output, -5000, 5000);
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash 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) static int16_t get_angle_boost(int16_t value)
{ {
float temp = cos_pitch_x * cos_roll_x; // float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0); // temp = 1.0 - constrain(temp, .5, 1.0);
int16_t output = temp * value; // int16_t output = temp * value;
return constrain(output, 0, 200); // return constrain(output, 0, 200);
// return (int)(temp * value); // 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 #if FRAME_CONFIG == HELI_FRAME

10
ArduCopter/config.h
View File

@ -654,26 +654,26 @@
// Stabilize Rate Control // Stabilize Rate Control
// //
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.14 # define RATE_ROLL_P 0.18
#endif #endif
#ifndef RATE_ROLL_I #ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0 # define RATE_ROLL_I 0.0
#endif #endif
#ifndef RATE_ROLL_D #ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.000 //.002 # define RATE_ROLL_D 0.004
#endif #endif
#ifndef RATE_ROLL_IMAX #ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 5 // degrees # define RATE_ROLL_IMAX 5 // degrees
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.14 # define RATE_PITCH_P 0.18
#endif #endif
#ifndef RATE_PITCH_I #ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.0 # define RATE_PITCH_I 0.0
#endif #endif
#ifndef RATE_PITCH_D #ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.00 # define RATE_PITCH_D 0.004
#endif #endif
#ifndef RATE_PITCH_IMAX #ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 5 // degrees # define RATE_PITCH_IMAX 5 // degrees
@ -694,7 +694,7 @@
#ifndef STABILIZE_D #ifndef STABILIZE_D
# define STABILIZE_D 0.15 # define STABILIZE_D 0.00
#endif #endif
#ifndef STABILIZE_D_SCHEDULE #ifndef STABILIZE_D_SCHEDULE