From ba8fc57bd8b51918cda5f8720fd38ba553494d2b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 12 Dec 2011 17:45:27 -0800 Subject: [PATCH] Stability patch2 - Jose style. --- ArduCopter/Attitude.pde | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 667fe557bb..96850ba02a 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -23,8 +23,11 @@ get_stabilize_roll(int32_t target_angle) // limit the error we're feeding to the PID error = constrain(error, -2500, 2500); + roll_I += (float)error * .03; + roll_I = constrain(roll_I,-120,120); //+- 1200 + // conver to desired Rate: - rate = g.pi_stabilize_roll.get_p(error); + rate = g.pi_stabilize_roll.get_p(error + roll_I); // experiment to pipe iterm directly into the output int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt); @@ -62,6 +65,12 @@ get_stabilize_pitch(int32_t target_angle) // angle error error = constrain(error, -2500, 2500); + pitch_I += (float)error * .03; + pitch_I = constrain(pitch_I, -120, 120); // +- 1200 + + // conver to desired Rate: + rate = g.pi_stabilize_roll.get_p(error + pitch_I); + // convert to desired Rate: rate = g.pi_stabilize_pitch.get_p(error);