Revision 0:2b4ec92bff8b, committed 2014-02-20

Comitter:

wisnup

Date:

Thu Feb 20 04:07:36 2014 +0000

Commit message:

dcm

Changed in this revision

--- /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;    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM.h	Thu Feb 20 04:07:36 2014 +0000
@@ -0,0 +1,130 @@
+#include "mbed.h"
+#include "Matrix.h"
+
+
+#define DEBUG__NO_DRIFT_CORRECTION true
+
+//Change this parameter if you wanna use another gyroscope. 
+//by default it is ITG3200 gain.
+#define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
+#define GYRO_GAIN (0.06956521739130434782608695652174)
+
+// DCM parameters
+#define Kp_ROLLPITCH 0.02f
+#define Ki_ROLLPITCH 0.00002f
+#define Kp_YAW 1.2f
+#define Ki_YAW 0.00002f
+
+#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
+#define TO_RAD(x) (x * 0.01745329252)  // *pi/180
+#define TO_DEG(x) (x * 57.2957795131)  // *180/pi
+
+#define MAGN_X_MAX 465
+#define MAGN_Y_MAX 475
+#define MAGN_Z_MAX 600
+
+#define MAGN_X_MIN -561
+#define MAGN_Y_MIN -547
+#define MAGN_Z_MIN -379
+
+#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
+#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
+#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
+
+#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
+#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
+#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+
+#define ACCEL_X_MIN ((float) -4096)
+#define ACCEL_X_MAX ((float) 4096)
+
+#define ACCEL_Y_MIN ((float) -4096)//267
+#define ACCEL_Y_MAX ((float) 4096)
+
+#define ACCEL_Z_MIN ((float) -4096)
+#define ACCEL_Z_MAX ((float) 4096)
+
+#define GYRO_BIAS_X 9.65       
+#define GYRO_BIAS_Y -5.05
+#define GYRO_BIAS_Z -8.37
+
+
+#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
+#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
+#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
+#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
+#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
+#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
+
+class DCM
+{
+public:
+    DCM(float dt);    
+    void UpdateFilter();    
+    void FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ); 
+    float getYaw();
+    float getPitch();
+    float getRoll();
+    void setAccelRawX(int accelRawX);
+    void setAccelRawY(int accelRawY);
+    void setAccelRawZ(int accelRawZ);
+    void setGyroRawX(int gyroRawX);
+    void setGyroRawY(int gyroRawY);
+    void setGyroRawZ(int gyroRawZ);
+    void setMagnetRawX(int magnetRawX);
+    void setMagnetRawY(int magnetRawY);
+    void setMagnetRawZ(int magnetRawZ);
+    float getTestVariable();
+      
+private:
+
+    void reset_sensor_fusion();
+    void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll);
+    void ReadMagnetometer();
+    void ReadAccelerometer();
+    void ReadGyroscope();    
+    float constrain(float in, float min, float max);
+    void Compass_Heading();
+    void Normalize(void);
+    void Drift_correction(void);
+    void Matrix_update(void);
+    void Euler_angles(void);
+    void Sensors();   
+    
+
+    // Sensor variables
+    float accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
+    int accelRaw[3];
+
+    float magnet[3];
+    int magnetRaw[3];
+
+    int gyro[3];
+    int gyroRaw[3];
+
+    int gyro_num_samples;
+
+    // DCM variables
+    float MAG_Heading;
+    float Accel_Vector[3];//= {0, 0, 0}; // Store the acceleration in a vector
+    float Gyro_Vector[3];//= {0, 0, 0}; // Store the gyros turn rate in a vector
+    float Omega_Vector[3];//= {0, 0, 0}; // Corrected Gyro_Vector data
+    float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
+    float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
+    float Omega[3];//= {0, 0, 0};
+    float errorRollPitch[3];// = {0, 0, 0};
+    float errorYaw[3];// = {0, 0, 0};
+    float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+    float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
+    float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+    // Euler angles
+    float yaw;
+    float pitch;
+    float roll;
+
+    float G_Dt; // Integration time for DCM algorithm
+
+    int rawMagnet[3];
+    int gyroBias[3];
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.cpp	Thu Feb 20 04:07:36 2014 +0000
@@ -0,0 +1,54 @@
+void Vector_Cross_Product(float C[3], float A[3], float B[3])
+{
+    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
+    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
+    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
+  
+    return;
+}
+
+void Vector_Scale(float C[3], float A[3], float b)
+{
+    for (int m = 0; m < 3; m++)
+        C[m] = A[m] * b;
+        
+    return;
+}
+
+float Vector_Dot_Product(float A[3], float B[3])
+{
+    float result = 0.0;
+
+    for (int i = 0; i < 3; i++) {
+        result += A[i] * B[i];
+    }
+    
+    return result;
+}
+
+void Vector_Add(float C[3], float A[3], float B[3])
+{
+    for (int m = 0; m < 3; m++)
+        C[m] = A[m] + B[m];
+        
+    return;
+}
+
+void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
+{
+    for (int m = 0; m < 3; m++)
+        for (int n = 0; n < 3; n++)
+            C[m][n] = A[m][n] + B[m][n];
+}
+
+void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
+{
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++) {
+            C[i][j] = 0;
+            for (int k = 0; k < 3; k++) {
+               C[i][j] += A[i][k] * B[k][j];
+            }
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.h	Thu Feb 20 04:07:36 2014 +0000
@@ -0,0 +1,39 @@
+#ifndef __MATRIX_H
+#define __MATRIX_H
+
+/** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut
+ * @param v1 is the first vector
+ * @param v2 is the second vector
+ * @returns vectorOut = v1 x v2
+ */
+void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]);
+
+/** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2
+ * @param vectorIn the vector
+ * @param scale2 is the scalar
+ * @returns vectorOut the result
+ */
+void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2);
+
+/** TDot product of two 3x1 vectors vector1 . vector2
+ * @param vector1 is the first vector
+ * @param vector2 is the second vector
+ * @returns float result of dot product
+ */
+float Vector_Dot_Product(float vector1[3], float vector2[3]);
+
+/** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2
+ * @param vectorIn1 is the first vector
+ * @param vectorIn2 is the second vector
+ * @returns vectorOut is the result of the addition
+ */
+void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]);
+
+/** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab
+ * @param a is the first vector
+ * @param b is the second vector
+ * @returns c as the result of the mutliplication
+ */
+void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]);
+
+#endif
\ No newline at end of file