Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Corrected bug on AccelVector and magnetometer support 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@4 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-05-31 20:13:22 +00:00
parent 3976200f50
commit 015b2599c6
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter.pde
View File

@ -401,7 +401,7 @@ void setup()
APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15); APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15);
APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15); APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15);
#if (MAGNETOMTER==1) #if (MAGNETOMETER)
APM_Compass.Init(); // I2C initialization APM_Compass.Init(); // I2C initialization
#endif #endif
@ -508,11 +508,12 @@ void loop(){
// IMU DCM Algorithm // IMU DCM Algorithm
Read_adc_raw(); Read_adc_raw();
#if (MAGNETOMETER==1) #if (MAGNETOMETER)
if (counter > 8) // Read compass data at 10Hz... (7 loop runs) if (counter > 8) // Read compass data at 10Hz... (7 loop runs)
{ {
counter=0; counter=0;
APM_Compass.Read(); // Read magnetometer APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading
} }
#endif #endif
Matrix_update(); Matrix_update();

6
DCM.pde
View File

@ -78,8 +78,8 @@ void Drift_correction(void)
//*****YAW*************** //*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading // We make the gyro YAW drift correction based on compass magnetic heading
#if (MAGNETOMETER==1) #if (MAGNETOMETER)
errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_X) - (DCM_Matrix[1][0]*APM_Compass.Heading_Y); //Calculating YAW error 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(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); 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[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[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[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional