From b7ecadf982bfcfd9f0b64275451d19a7cbd67f54 Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Tue, 12 Oct 2010 08:44:20 +0000 Subject: [PATCH] Added limit to errorYaw in DCM to prevent I term to grow too much git-svn-id: https://arducopter.googlecode.com/svn/trunk@656 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArducopterNG/ArduUser.h | 16 +++++++--------- ArducopterNG/DCM.pde | 5 +++++ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index eccebc288d..ccaf4f72fa 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -33,7 +33,6 @@ TODO: * ************************************************************** */ - /*************************************************************/ // Safety & Security @@ -41,7 +40,6 @@ TODO: #define ARM_DELAY 200 // milliseconds of how long you need to keep rudder to max right for arming motors #define DISARM_DELAY 100 // milliseconds of how long you need to keep rudder to max left for disarming motors - /*************************************************************/ // AM Mode & Flight information @@ -181,16 +179,16 @@ void defaultUserConfig() { KP_ALTITUDE = 0.8; KI_ALTITUDE = 0.2; KD_ALTITUDE = 0.2; - acc_offset_x = 2073; - acc_offset_y = 2056; - acc_offset_z = 2010; + acc_offset_x = 2048; + acc_offset_y = 2048; + acc_offset_z = 2048; gyro_offset_roll = 1659; - gyro_offset_pitch = 1618; - gyro_offset_yaw = 1673; + gyro_offset_pitch = 1650; + gyro_offset_yaw = 1650; Kp_ROLLPITCH = 0.0014; Ki_ROLLPITCH = 0.00000015; - Kp_YAW = 1.2; - Ki_YAW = 0.00005; + Kp_YAW = 1.0; + Ki_YAW = 0.00002; GEOG_CORRECTION_FACTOR = 0.87; MAGNETOMETER = 0; Kp_RateRoll = 1.95; diff --git a/ArducopterNG/DCM.pde b/ArducopterNG/DCM.pde index f3d59a2bde..bbeea87185 100644 --- a/ArducopterNG/DCM.pde +++ b/ArducopterNG/DCM.pde @@ -92,6 +92,11 @@ void Drift_correction(void) Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. + + // Limit max errorYaw to limit max Omega_I + errorYaw[0] = constrain(errorYaw[0],-50,50); + errorYaw[1] = constrain(errorYaw[1],-50,50); + errorYaw[2] = constrain(errorYaw[2],-50,50); Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I