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

Stability patch2 - Jose style.

This commit is contained in:
Jason Short 2011-12-12 17:45:27 -08:00
parent c9b317fc27
commit ba8fc57bd8
1 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/Attitude.pde
View File

@ -23,8 +23,11 @@ get_stabilize_roll(int32_t target_angle)
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
roll_I += (float)error * .03;
roll_I = constrain(roll_I,-120,120); //+- 1200
// conver to desired Rate: // 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 // experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt); 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 // angle error
error = constrain(error, -2500, 2500); 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: // convert to desired Rate:
rate = g.pi_stabilize_pitch.get_p(error); rate = g.pi_stabilize_pitch.get_p(error);