From b1d47ef1357c2c241742127a9d3aef3dfb546093 Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Sat, 13 Aug 2011 09:37:01 +0000 Subject: [PATCH] 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 --- ArduCopterMega/Attitude.pde | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index eec0fcd12f..2b793c6299 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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); //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: error = rate - (long)(degrees(omega.x) * 100.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); +#endif // output control: - return (int)constrain(rate, -2500, 2500); + return (int)constrain(rate, -2500, 2500); + } 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); //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: error = rate - (long)(degrees(omega.y) * 100.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); +#endif // output control: 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); //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: error = rate - (long)(degrees(omega.z) * 100.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); +#endif // output control: return (int)constrain(rate, -2500, 2500);