Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues 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);
#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

10
ArduCopter/config.h
View File

@ -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