From eaef5315bf73780be4b02c72fbf781b6a278c260 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 6 Nov 2013 14:47:06 +0900 Subject: [PATCH] TradHeli: bugfix to update swash in run_rate_controllers --- ArduCopter/Attitude.pde | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 3c73683065..eed103e5d2 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -442,10 +442,15 @@ update_rate_contoller_targets() void run_rate_controllers() { -#if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw and only when not using an external gyro +#if FRAME_CONFIG == HELI_FRAME + // convert desired roll and pitch rate to roll and pitch swash angles + heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf); + // helicopters only use rate controllers for yaw and only when not using an external gyro if(!motors.ext_gyro_enabled()) { - heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf); g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf); + }else{ + // do not use rate controllers for helicotpers with external gyros + g.rc_4.servo_out = constrain_int32(yaw_rate_target_bf, -4500, 4500); } #else // call rate controllers