Diff: DCM.cpp

Revision:

0:2b4ec92bff8b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM.cpp	Thu Feb 20 04:07:36 2014 +0000
@@ -0,0 +1,344 @@
+#include "DCM.h"
+
+DCM::DCM(float dt)
+{
+    for(int i = 0; i < 3; i++)
+    {
+        Accel_Vector[i] = 0; // Store the acceleration in a vector
+        Gyro_Vector[i] = 0; // Store the gyros turn rate in a vector
+        Omega_Vector[i] = 0; // Corrected Gyro_Vector data
+        Omega_P[i] = 0; // Omega Proportional correction
+        Omega_I[i] = 0; // Omega Integrator
+        Omega[i]= 0;
+        errorRollPitch[i] = 0;
+        errorYaw[i] = 0;
+    }
+    
+    int k = 1;
+    
+    for(int i = 0; i < 3; i++)
+    {
+        for(int j = 0; j < 3; j++)
+        {
+            DCM_Matrix[i][j] = 0;
+            Update_Matrix[i][j] = k;
+            Temporary_Matrix[i][j] = 0;
+            k++;
+        }
+        k++;
+    }
+    
+    G_Dt = dt;    
+    
+    reset_sensor_fusion();    
+}
+
+void DCM::FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ)
+{
+    setAccelRawX(rawAccX);
+    setAccelRawY(rawAccY);
+    setAccelRawZ(rawAccZ);
+    
+    setGyroRawX(rawGyroX);
+    setGyroRawY(rawGyroY);
+    setGyroRawZ(rawGyroZ);
+    
+    setMagnetRawX(rawMagX);
+    setMagnetRawY(rawMagY);
+    setMagnetRawZ(rawMagZ);
+    
+    reset_sensor_fusion();
+}
+
+void DCM::init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
+{
+  float c1 = cos(roll);
+  float s1 = sin(roll);
+  float c2 = cos(pitch);
+  float s2 = sin(pitch);
+  float c3 = cos(yaw);
+  float s3 = sin(yaw);
+
+  // Euler angles, right-handed, intrinsic, XYZ convention
+  // (which means: rotate around body axes Z, Y', X'') 
+  m[0][0] = c2 * c3;
+  m[0][1] = c3 * s1 * s2 - c1 * s3;
+  m[0][2] = s1 * s3 + c1 * c3 * s2;
+
+  m[1][0] = c2 * s3;
+  m[1][1] = c1 * c3 + s1 * s2 * s3;
+  m[1][2] = c1 * s2 * s3 - c3 * s1;
+
+  m[2][0] = -s2;
+  m[2][1] = c2 * s1;
+  m[2][2] = c1 * c2;
+}
+
+void DCM::ReadMagnetometer()
+{
+    rawMagnet[0] = ((int16_t)magnetRaw[0]);
+    rawMagnet[1] = ((int16_t)magnetRaw[1]);
+    rawMagnet[2] = ((int16_t)magnetRaw[2]);
+    
+    magnet[0] = ((float)(rawMagnet[0] - MAGN_X_OFFSET) * MAGN_X_SCALE);
+    magnet[1] = ((float)(rawMagnet[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE);
+    magnet[2] = ((float)(rawMagnet[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE);
+}
+
+void DCM::ReadAccelerometer()
+{
+    accel[0] = (((int16_t)accelRaw[0]) - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    accel[1] = (((int16_t)accelRaw[1]) - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    accel[2] = (((int16_t)accelRaw[2]) - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+}
+
+void DCM::ReadGyroscope()
+{
+    gyro[0] = (gyroRaw[0] - GYRO_BIAS_X);
+    gyro[1] = (gyroRaw[1] - GYRO_BIAS_Y);   
+    gyro[2] = (gyroRaw[2] - GYRO_BIAS_Z);       
+}
+
+float DCM::constrain(float in, float min, float max)
+{
+    in = in > max ? max : in;
+    in = in < min ? min : in;
+    return in;
+}
+
+void DCM::Compass_Heading()
+{
+  float mag_x;
+  float mag_y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // Tilt compensated magnetic field X
+  mag_x = magnet[0]*cos_pitch + magnet[1]*sin_roll*sin_pitch + magnet[2]*cos_roll*sin_pitch;
+  // Tilt compensated magnetic field Y
+  mag_y = magnet[1]*cos_roll - magnet[2]*sin_roll;
+  // Magnetic Heading
+  MAG_Heading = atan2(-mag_y, mag_x);
+}
+
+void DCM::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 DCM::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 DCM::Matrix_update(void)
+{
+  Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
+  Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
+  Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
+  
+  Accel_Vector[0]=accel[0];
+  Accel_Vector[1]=accel[1];
+  Accel_Vector[2]=accel[2];
+    
+  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];
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];
+  Update_Matrix[2][2]=0;
+
+  Matrix_Multiply(Temporary_Matrix,DCM_Matrix,Update_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::Sensors()
+{
+    ReadAccelerometer();
+    ReadGyroscope();
+    ReadMagnetometer();
+    Compass_Heading();    
+}
+
+void DCM::UpdateFilter()
+{
+    Sensors();
+    Matrix_update();
+    Normalize();
+    Drift_correction(); 
+    Euler_angles();    
+}
+
+void DCM::reset_sensor_fusion() {
+  float temp1[3];
+  float temp2[3];
+  float xAxis[] = {1.0f, 0.0f, 0.0f};
+
+  Sensors();
+    
+  // GET PITCH
+  // Using y-z-plane-component/x-component of gravity vector
+  pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));
+    
+  Vector_Cross_Product(temp1, accel, xAxis);
+  Vector_Cross_Product(temp2, xAxis, temp1);
+  roll = atan2(temp2[1], temp2[2]);
+  
+  // GET YAW
+  Compass_Heading();
+  yaw = MAG_Heading;
+  
+  // Init rotation matrix
+  init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
+}
+
+float DCM::getYaw()
+{
+    return TO_DEG(yaw);
+}
+
+float DCM::getPitch()
+{
+    return TO_DEG(pitch);
+}
+
+float DCM::getRoll()
+{
+    return TO_DEG(roll);
+}
+
+void DCM::setAccelRawX(int accelRawX)
+{
+    accelRaw[0] = (uint16_t)accelRawX;    
+}
+
+void DCM::setAccelRawY(int accelRawY)
+{
+    accelRaw[1] = (uint16_t)accelRawY;    
+}
+
+void DCM::setAccelRawZ(int accelRawZ)
+{
+    accelRaw[2] = (uint16_t)accelRawZ;    
+}
+
+void DCM::setGyroRawX(int gyroRawX)
+{
+    gyroRaw[0] = gyroRawX;    
+}
+
+void DCM::setGyroRawY(int gyroRawY)
+{
+    gyroRaw[1] = gyroRawY;    
+}
+
+void DCM::setGyroRawZ(int gyroRawZ)
+{
+    gyroRaw[2] = gyroRawZ;    
+}
+
+void DCM::setMagnetRawX(int magnetRawX)
+{
+    magnetRaw[0] = (uint16_t)magnetRawX;    
+}
+
+void DCM::setMagnetRawY(int magnetRawY)
+{
+    magnetRaw[1] = (uint16_t)magnetRawY;    
+}
+
+void DCM::setMagnetRawZ(int magnetRawZ)
+{
+    magnetRaw[2] = (uint16_t)magnetRawZ;    
+}
+
+float DCM::getTestVariable()
+{
+    return MAG_Heading;    
+}