Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

cleanup

This commit is contained in:
Jason Short 2012-01-20 21:49:15 -08:00
parent a087d84734
commit 525f7cd40f
1 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/Attitude.pde
View File

@ -8,8 +8,6 @@ get_stabilize_roll(int32_t target_angle)
static float current_rate = 0; static float current_rate = 0;
current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3;
// angle error // angle error
error = wrap_180(target_angle - dcm.roll_sensor); error = wrap_180(target_angle - dcm.roll_sensor);
@ -38,6 +36,7 @@ get_stabilize_roll(int32_t target_angle)
rate = g.pi_rate_roll.get_pi(error, G_Dt); rate = g.pi_rate_roll.get_pi(error, G_Dt);
// D term // D term
current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3;
int16_t d_temp = current_rate * g.stablize_d; int16_t d_temp = current_rate * g.stablize_d;
rate -= d_temp; rate -= d_temp;
@ -54,9 +53,6 @@ get_stabilize_pitch(int32_t target_angle)
int32_t rate = 0; int32_t rate = 0;
static float current_rate = 0; static float current_rate = 0;
//current_rate = (omega.y * DEGX100);
current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3;
// angle error // angle error
error = wrap_180(target_angle - dcm.pitch_sensor); error = wrap_180(target_angle - dcm.pitch_sensor);
@ -87,6 +83,7 @@ get_stabilize_pitch(int32_t target_angle)
rate = g.pi_rate_pitch.get_pi(error, G_Dt); rate = g.pi_rate_pitch.get_pi(error, G_Dt);
// D term // D term
current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3;
int16_t d_temp = current_rate * g.stablize_d; int16_t d_temp = current_rate * g.stablize_d;
rate -= d_temp; rate -= d_temp;
@ -324,6 +321,8 @@ static int get_angle_boost(int 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;
// return constrain(output, 0, 100);
return (int)(temp * value); return (int)(temp * value);
} }