From e9861de2ac81edf3d08071ef6f95c49dd65dd526 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 23 Jul 2014 17:11:55 +0900 Subject: [PATCH] Copter: fix to drift's yaw rate The ACRO_P parameter needs to be multiplied by the roll and pitch velocities to calculate the final yaw rate --- ArduCopter/control_drift.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index f7bb079fe6..c3991a86a4 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -66,7 +66,7 @@ static void drift_run() float pitch_vel2 = min(fabs(pitch_vel), 800); // simple gain scheduling for yaw input - target_yaw_rate = (float)(target_roll/2) * (1.0 - (pitch_vel2 / 2400.0)); + target_yaw_rate = (float)(target_roll/2.0f) * (1.0f - (pitch_vel2 / 2400.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -322, 322); pitch_vel = constrain_float(pitch_vel, -322, 322);