From 42b1362bba572cf28ee34925d0be64286ccec6de Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 20 Jan 2012 21:49:15 -0800 Subject: [PATCH] cleanup --- ArduCopter/Attitude.pde | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 51c8a049ed..d6efca9a10 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -8,8 +8,6 @@ get_stabilize_roll(int32_t target_angle) static float current_rate = 0; - current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3; - // angle error error = wrap_180(target_angle - dcm.roll_sensor); @@ -38,7 +36,8 @@ get_stabilize_roll(int32_t target_angle) rate = g.pi_rate_roll.get_pi(error, G_Dt); // D term - int16_t d_temp = current_rate * g.stablize_d; + current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3; + int16_t d_temp = current_rate * g.stablize_d; rate -= d_temp; // output control: @@ -54,9 +53,6 @@ get_stabilize_pitch(int32_t target_angle) int32_t rate = 0; static float current_rate = 0; - //current_rate = (omega.y * DEGX100); - current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3; - // angle error error = wrap_180(target_angle - dcm.pitch_sensor); @@ -87,7 +83,8 @@ get_stabilize_pitch(int32_t target_angle) rate = g.pi_rate_pitch.get_pi(error, G_Dt); // D term - int16_t d_temp = current_rate * g.stablize_d; + current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3; + int16_t d_temp = current_rate * g.stablize_d; rate -= d_temp; // output control: @@ -324,6 +321,8 @@ static int get_angle_boost(int 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, 100); return (int)(temp * value); }