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

TradHeli - fixes to use single PI controller for roll, pitch and yaw (when using an external tail gyro) 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3089 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2011-08-13 09:37:01 +00:00
parent e95d0eb686
commit 37dd9ac144
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopterMega/Attitude.pde
View File

@ -16,13 +16,16 @@ get_stabilize_roll(long target_angle)
rate = g.pid_stabilize_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pid_stabilize_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.x) * 100.0); error = rate - (long)(degrees(omega.x) * 100.0);
rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control: // output control:
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
static int static int
@ -40,10 +43,12 @@ get_stabilize_pitch(long target_angle)
rate = g.pid_stabilize_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pid_stabilize_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.y) * 100.0); error = rate - (long)(degrees(omega.y) * 100.0);
rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control: // output control:
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
@ -65,10 +70,18 @@ get_stabilize_yaw(long target_angle, float scaler)
rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler); rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler);
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate); //Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
if( ! g.heli_ext_gyro_enabled ) {
// Rate P:
error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0);
}
#else
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.z) * 100.0); error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control: // output control:
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);