1 files changed, 0 insertions, 164 deletions
diff --git a/CODE/debug/prewk/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino b/CODE/debug/prewk/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino
deleted file mode 100644
index 7f9f276..0000000
--- a/CODE/debug/prewk/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
-
-MinIMU-9-Arduino-AHRS
-Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
-
-Copyright (c) 2011-2016 Pololu Corporation.
-http://www.pololu.com/
-
-MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
-http://code.google.com/p/sf9domahrs/
-
-sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
-Julio and Doug Weibel:
-http://code.google.com/p/ardu-imu/
-
-MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
-under the terms of the GNU Lesser General Public License as published by the
-Free Software Foundation, either version 3 of the License, or (at your option)
-any later version.
-
-MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
-WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
-more details.
-
-You should have received a copy of the GNU Lesser General Public License along
-with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
-
-*/
-
-/**************************************************/
-void Normalize(void)
-{
-  float error=0;
-  float temporary[3][3];
-  float renorm=0;
-  
-  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
-
-  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
-  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
-  
-  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
-  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
-  
-  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
-  
-  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
-  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
-  
-  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
-  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
-  
-  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
-  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
-}
-
-/**************************************************/
-void Drift_correction(void)
-{
-  float mag_heading_x;
-  float mag_heading_y;
-  float errorCourse;
-  //Compensation the Roll, Pitch and Yaw drift. 
-  static float Scaled_Omega_P[3];
-  static float Scaled_Omega_I[3];
-  float Accel_magnitude;
-  float Accel_weight;
-  
-  
-  //*****Roll and Pitch***************
-
-  // Calculate the magnitude of the accelerometer vector
-  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
-  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
-  // Dynamic weighting of accelerometer info (reliability filter)
-  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
-  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
-
-  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
-  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
-  
-  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
-  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
-  
-  //*****YAW***************
-  // We make the gyro YAW drift correction based on compass magnetic heading
- 
-  mag_heading_x = cos(MAG_Heading);
-  mag_heading_y = sin(MAG_Heading);
-  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_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);//.01proportional of YAW.
-  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
-  
-  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
-  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
-}
-/**************************************************/
-/*
-void Accel_adjust(void)
-{
- Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
- Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY 
-}
-*/
-/**************************************************/
-
-void Matrix_update(void)
-{
-  Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
-  Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
-  Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
-  
-  Accel_Vector[0]=accel_x;
-  Accel_Vector[1]=accel_y;
-  Accel_Vector[2]=accel_z;
-    
-  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
-  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
-
-  //Accel_adjust();    //Remove centrifugal acceleration.   We are not using this function in this version - we have no speed measurement
-  
- #if OUTPUTMODE==1         
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
-  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
-  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
-  Update_Matrix[2][2]=0;
- #else                    // Uncorrected data (no drift correction)
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
-  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][2]=0;
- #endif
-
-  Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
-
-  for(int x=0; x<3; x++) //Matrix Addition (update)
-  {
-    for(int y=0; y<3; y++)
-    {
-      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
-    } 
-  }
-}
-
-void Euler_angles(void)
-{
-  pitch = -asin(DCM_Matrix[2][0]);
-  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
-  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
-}
-