diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0ace4409a4..cac6eeae78 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -824,8 +824,8 @@ void loop() uint32_t timer = micros(); // We want this to execute fast // ---------------------------- - if ((timer - fast_loopTimer) >= 5000) { - Log_Write_Data(13, (int32_t)(timer - fast_loopTimer)); + if ((timer - fast_loopTimer) >= 4000) { + //Log_Write_Data(13, (int32_t)(timer - fast_loopTimer)); //PORTK |= B00010000; G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 502e868233..434a090335 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -144,6 +144,7 @@ // Rate #define CH6_RATE_KP 4 #define CH6_RATE_KI 5 +#define CH6_RATE_KD 21 #define CH6_YAW_RATE_KP 6 // Altitude rate controller #define CH6_THROTTLE_KP 7