From 870b5e5f75b09e0719da24c7fef29656c3920c51 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Mon, 16 Apr 2012 23:47:57 +0900 Subject: [PATCH] ArduCopter - CH6 Tuning - added AHRS_YAW_KP parameter as 30. Now you can easily set the amount that the mag is used to correct the gyro based yaw estimate in the DCM --- ArduCopter/ArduCopter.pde | 4 ++++ ArduCopter/defines.h | 2 ++ 2 files changed, 6 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e477fdedef..e29e588fa4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2166,6 +2166,10 @@ static void tuning(){ g.pid_optflow_roll.kD(tuning_value); g.pid_optflow_pitch.kD(tuning_value); break; + + case CH6_AHRS_YAW_KP: + ahrs._kp_yaw.set(tuning_value); + break; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 12a722bc97..693f429870 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -184,6 +184,8 @@ #define CH6_LOITER_RATE_KI 28 #define CH6_LOITER_RATE_KD 23 +#define CH6_AHRS_YAW_KP 30 + // nav byte mask // -------------