Diff: DCM/DCM.cpp

Revision:

1:e08a4f517989

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM/DCM.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,197 @@
+#include"DCM.h"
+#include "HelperMath.h"
+
+float DCM::constrain(float x, float a, float b) 
+{
+    float result = x;
+    
+    if (x < a) result = a;
+    if (x > b) result = b;
+ 
+    return result;  
+}
+
+DCM::DCM(){
+
+  G_Dt=0.005;
+  
+  for(int i=0;i<3;i++){
+  Accel_Vector[i]=0;
+  Gyro_Vector[i]= 0;
+  Omega_Vector[i]=0;
+  Omega_P[i]=0;
+  Omega_I[i]=0;
+  Omega[i]= 0;
+  errorRollPitch[i]=0;
+  }
+  
+  DCM_Matrix[0][0]=1;
+  DCM_Matrix[0][1]=0;
+  DCM_Matrix[0][2]=0;
+  DCM_Matrix[1][0]=0;
+  DCM_Matrix[1][1]=1;
+  DCM_Matrix[1][2]=0;
+  DCM_Matrix[2][0]=0;
+  DCM_Matrix[2][1]=0;
+  DCM_Matrix[2][2]=1;
+  
+  Temporary_Matrix[0][0]=0;
+  Temporary_Matrix[0][1]=0;
+  Temporary_Matrix[0][2]=0;
+  Temporary_Matrix[1][0]=0;
+  Temporary_Matrix[1][1]=0;
+  Temporary_Matrix[1][2]=0;
+  Temporary_Matrix[2][0]=0;
+  Temporary_Matrix[2][1]=0;
+  Temporary_Matrix[2][2]=0;
+  
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=1;
+  Update_Matrix[0][2]=2;
+  Update_Matrix[1][0]=3;
+  Update_Matrix[1][1]=4;
+  Update_Matrix[1][2]=5;
+  Update_Matrix[2][0]=6;
+  Update_Matrix[2][1]=7;
+  Update_Matrix[2][2]=8;
+}
+/**************************************************/
+void DCM::Normalize(void)
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+  bool problem=false;
+  
+  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= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); 
+  if (renorm < 1.5625f && renorm > 0.64f) {
+    renorm= .5 * (3-renorm);                                                 //eq.21
+  } else if (renorm < 100.0f && renorm > 0.01f) {
+    renorm= 1. / sqrt(renorm);
+  }
+      Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+  
+  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); 
+  if (renorm < 1.5625f && renorm > 0.64f) {
+    renorm= .5 * (3-renorm);                                                 //eq.21
+  } else if (renorm < 100.0f && renorm > 0.01f) {
+    renorm= 1. / sqrt(renorm);  
+  } else {
+    problem = true;
+  }
+  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+  
+  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); 
+  if (renorm < 1.5625f && renorm > 0.64f) {
+    renorm= .5 * (3-renorm);                                                 //eq.21
+  } else if (renorm < 100.0f && renorm > 0.01f) {
+    renorm= 1. / sqrt(renorm);   
+
+  } else {
+    problem = true;  
+  }
+  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+  
+  if (problem) {                // Our solution is blowing up and we will force back to initial condition.  Hope we are not upside down!
+      DCM_Matrix[0][0]= 1.0f;
+      DCM_Matrix[0][1]= 0.0f;
+      DCM_Matrix[0][2]= 0.0f;
+      DCM_Matrix[1][0]= 0.0f;
+      DCM_Matrix[1][1]= 1.0f;
+      DCM_Matrix[1][2]= 0.0f;
+      DCM_Matrix[2][0]= 0.0f;
+      DCM_Matrix[2][1]= 0.0f;
+      DCM_Matrix[2][2]= 1.0f;
+      problem = false;  
+  }
+}
+
+/**************************************************/
+void DCM::Drift_correction(void)
+{
+  //Compensation the Roll, Pitch and Yaw drift. 
+  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 /=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);     
+  
+}
+
+void DCM::Matrix_update(float GyroX,float GyroY,float GyroZ, float AccX, float AccY, float AccZ)
+{
+  Gyro_Vector[0]=GyroX; //gyro x roll
+  Gyro_Vector[1]=GyroY; //gyro y pitch
+  Gyro_Vector[2]=GyroZ; //gyro Z yaw
+  
+  Accel_Vector[0]=AccX*GRAVITY; // acc x
+  Accel_Vector[1]=AccY*GRAVITY; // acc y
+  Accel_Vector[2]=AccZ*GRAVITY; // acc 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
+       
+  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;
+
+  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 DCM::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]);
+}
+
+void DCM::Update_DCM(float dt,float a, float b, float c, float d, float e, float f){
+         
+         G_Dt = dt;
+         
+         Matrix_update(a,b,c,d,e,f); 
+
+         Normalize();
+
+         Drift_correction();
+   
+         Euler_angles();
+}
\ No newline at end of file