From 015b2599c68b8373f3b22938260e45d4cbda870e Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Mon, 31 May 2010 20:13:22 +0000 Subject: [PATCH] Corrected bug on AccelVector and magnetometer support git-svn-id: https://arducopter.googlecode.com/svn/trunk@4 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopter.pde | 5 +++-- DCM.pde | 6 ++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ArduCopter.pde b/ArduCopter.pde index e5298886be..a651418535 100644 --- a/ArduCopter.pde +++ b/ArduCopter.pde @@ -401,7 +401,7 @@ void setup() APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15); APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15); - #if (MAGNETOMTER==1) + #if (MAGNETOMETER) APM_Compass.Init(); // I2C initialization #endif @@ -508,11 +508,12 @@ void loop(){ // IMU DCM Algorithm Read_adc_raw(); - #if (MAGNETOMETER==1) + #if (MAGNETOMETER) if (counter > 8) // Read compass data at 10Hz... (7 loop runs) { counter=0; APM_Compass.Read(); // Read magnetometer + APM_Compass.Calculate(roll,pitch); // Calculate heading } #endif Matrix_update(); diff --git a/DCM.pde b/DCM.pde index 993f814ff4..edd52a181e 100644 --- a/DCM.pde +++ b/DCM.pde @@ -78,8 +78,8 @@ void Drift_correction(void) //*****YAW*************** // We make the gyro YAW drift correction based on compass magnetic heading - #if (MAGNETOMETER==1) - errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_X) - (DCM_Matrix[1][0]*APM_Compass.Heading_Y); //Calculating YAW error + #if (MAGNETOMETER) + errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); @@ -113,8 +113,6 @@ void Matrix_update(void) Accel_Vector[1]=Accel_Vector[1]*0.4 + (float)read_adc(4)*0.6; // acc y Accel_Vector[2]=Accel_Vector[2]*0.4 + (float)read_adc(5)*0.6; // acc z - Accel_Vector[1]=0; - Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional