wisnu pamungkas / DCM

dcm

DCM.h@0:2b4ec92bff8b, 2014-02-20 (annotated)

Committer:
wisnup
Date:
Thu Feb 20 04:07:36 2014 +0000
Revision:
0:2b4ec92bff8b
dcm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wisnup 0:2b4ec92bff8b 1 #include "mbed.h"
wisnup 0:2b4ec92bff8b 2 #include "Matrix.h"
wisnup 0:2b4ec92bff8b 3
wisnup 0:2b4ec92bff8b 4
wisnup 0:2b4ec92bff8b 5 #define DEBUG__NO_DRIFT_CORRECTION true
wisnup 0:2b4ec92bff8b 6
wisnup 0:2b4ec92bff8b 7 //Change this parameter if you wanna use another gyroscope.
wisnup 0:2b4ec92bff8b 8 //by default it is ITG3200 gain.
wisnup 0:2b4ec92bff8b 9 #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
wisnup 0:2b4ec92bff8b 10 #define GYRO_GAIN (0.06956521739130434782608695652174)
wisnup 0:2b4ec92bff8b 11
wisnup 0:2b4ec92bff8b 12 // DCM parameters
wisnup 0:2b4ec92bff8b 13 #define Kp_ROLLPITCH 0.02f
wisnup 0:2b4ec92bff8b 14 #define Ki_ROLLPITCH 0.00002f
wisnup 0:2b4ec92bff8b 15 #define Kp_YAW 1.2f
wisnup 0:2b4ec92bff8b 16 #define Ki_YAW 0.00002f
wisnup 0:2b4ec92bff8b 17
wisnup 0:2b4ec92bff8b 18 #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
wisnup 0:2b4ec92bff8b 19 #define TO_RAD(x) (x * 0.01745329252) // *pi/180
wisnup 0:2b4ec92bff8b 20 #define TO_DEG(x) (x * 57.2957795131) // *180/pi
wisnup 0:2b4ec92bff8b 21
wisnup 0:2b4ec92bff8b 22 #define MAGN_X_MAX 465
wisnup 0:2b4ec92bff8b 23 #define MAGN_Y_MAX 475
wisnup 0:2b4ec92bff8b 24 #define MAGN_Z_MAX 600
wisnup 0:2b4ec92bff8b 25
wisnup 0:2b4ec92bff8b 26 #define MAGN_X_MIN -561
wisnup 0:2b4ec92bff8b 27 #define MAGN_Y_MIN -547
wisnup 0:2b4ec92bff8b 28 #define MAGN_Z_MIN -379
wisnup 0:2b4ec92bff8b 29
wisnup 0:2b4ec92bff8b 30 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
wisnup 0:2b4ec92bff8b 31 #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
wisnup 0:2b4ec92bff8b 32 #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
wisnup 0:2b4ec92bff8b 33
wisnup 0:2b4ec92bff8b 34 #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
wisnup 0:2b4ec92bff8b 35 #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
wisnup 0:2b4ec92bff8b 36 #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
wisnup 0:2b4ec92bff8b 37
wisnup 0:2b4ec92bff8b 38 #define ACCEL_X_MIN ((float) -4096)
wisnup 0:2b4ec92bff8b 39 #define ACCEL_X_MAX ((float) 4096)
wisnup 0:2b4ec92bff8b 40
wisnup 0:2b4ec92bff8b 41 #define ACCEL_Y_MIN ((float) -4096)//267
wisnup 0:2b4ec92bff8b 42 #define ACCEL_Y_MAX ((float) 4096)
wisnup 0:2b4ec92bff8b 43
wisnup 0:2b4ec92bff8b 44 #define ACCEL_Z_MIN ((float) -4096)
wisnup 0:2b4ec92bff8b 45 #define ACCEL_Z_MAX ((float) 4096)
wisnup 0:2b4ec92bff8b 46
wisnup 0:2b4ec92bff8b 47 #define GYRO_BIAS_X 9.65
wisnup 0:2b4ec92bff8b 48 #define GYRO_BIAS_Y -5.05
wisnup 0:2b4ec92bff8b 49 #define GYRO_BIAS_Z -8.37
wisnup 0:2b4ec92bff8b 50
wisnup 0:2b4ec92bff8b 51
wisnup 0:2b4ec92bff8b 52 #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
wisnup 0:2b4ec92bff8b 53 #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
wisnup 0:2b4ec92bff8b 54 #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
wisnup 0:2b4ec92bff8b 55 #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
wisnup 0:2b4ec92bff8b 56 #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
wisnup 0:2b4ec92bff8b 57 #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
wisnup 0:2b4ec92bff8b 58
wisnup 0:2b4ec92bff8b 59 class DCM
wisnup 0:2b4ec92bff8b 60 {
wisnup 0:2b4ec92bff8b 61 public:
wisnup 0:2b4ec92bff8b 62 DCM(float dt);
wisnup 0:2b4ec92bff8b 63 void UpdateFilter();
wisnup 0:2b4ec92bff8b 64 void FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ);
wisnup 0:2b4ec92bff8b 65 float getYaw();
wisnup 0:2b4ec92bff8b 66 float getPitch();
wisnup 0:2b4ec92bff8b 67 float getRoll();
wisnup 0:2b4ec92bff8b 68 void setAccelRawX(int accelRawX);
wisnup 0:2b4ec92bff8b 69 void setAccelRawY(int accelRawY);
wisnup 0:2b4ec92bff8b 70 void setAccelRawZ(int accelRawZ);
wisnup 0:2b4ec92bff8b 71 void setGyroRawX(int gyroRawX);
wisnup 0:2b4ec92bff8b 72 void setGyroRawY(int gyroRawY);
wisnup 0:2b4ec92bff8b 73 void setGyroRawZ(int gyroRawZ);
wisnup 0:2b4ec92bff8b 74 void setMagnetRawX(int magnetRawX);
wisnup 0:2b4ec92bff8b 75 void setMagnetRawY(int magnetRawY);
wisnup 0:2b4ec92bff8b 76 void setMagnetRawZ(int magnetRawZ);
wisnup 0:2b4ec92bff8b 77 float getTestVariable();
wisnup 0:2b4ec92bff8b 78
wisnup 0:2b4ec92bff8b 79 private:
wisnup 0:2b4ec92bff8b 80
wisnup 0:2b4ec92bff8b 81 void reset_sensor_fusion();
wisnup 0:2b4ec92bff8b 82 void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll);
wisnup 0:2b4ec92bff8b 83 void ReadMagnetometer();
wisnup 0:2b4ec92bff8b 84 void ReadAccelerometer();
wisnup 0:2b4ec92bff8b 85 void ReadGyroscope();
wisnup 0:2b4ec92bff8b 86 float constrain(float in, float min, float max);
wisnup 0:2b4ec92bff8b 87 void Compass_Heading();
wisnup 0:2b4ec92bff8b 88 void Normalize(void);
wisnup 0:2b4ec92bff8b 89 void Drift_correction(void);
wisnup 0:2b4ec92bff8b 90 void Matrix_update(void);
wisnup 0:2b4ec92bff8b 91 void Euler_angles(void);
wisnup 0:2b4ec92bff8b 92 void Sensors();
wisnup 0:2b4ec92bff8b 93
wisnup 0:2b4ec92bff8b 94
wisnup 0:2b4ec92bff8b 95 // Sensor variables
wisnup 0:2b4ec92bff8b 96 float accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
wisnup 0:2b4ec92bff8b 97 int accelRaw[3];
wisnup 0:2b4ec92bff8b 98
wisnup 0:2b4ec92bff8b 99 float magnet[3];
wisnup 0:2b4ec92bff8b 100 int magnetRaw[3];
wisnup 0:2b4ec92bff8b 101
wisnup 0:2b4ec92bff8b 102 int gyro[3];
wisnup 0:2b4ec92bff8b 103 int gyroRaw[3];
wisnup 0:2b4ec92bff8b 104
wisnup 0:2b4ec92bff8b 105 int gyro_num_samples;
wisnup 0:2b4ec92bff8b 106
wisnup 0:2b4ec92bff8b 107 // DCM variables
wisnup 0:2b4ec92bff8b 108 float MAG_Heading;
wisnup 0:2b4ec92bff8b 109 float Accel_Vector[3];//= {0, 0, 0}; // Store the acceleration in a vector
wisnup 0:2b4ec92bff8b 110 float Gyro_Vector[3];//= {0, 0, 0}; // Store the gyros turn rate in a vector
wisnup 0:2b4ec92bff8b 111 float Omega_Vector[3];//= {0, 0, 0}; // Corrected Gyro_Vector data
wisnup 0:2b4ec92bff8b 112 float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
wisnup 0:2b4ec92bff8b 113 float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
wisnup 0:2b4ec92bff8b 114 float Omega[3];//= {0, 0, 0};
wisnup 0:2b4ec92bff8b 115 float errorRollPitch[3];// = {0, 0, 0};
wisnup 0:2b4ec92bff8b 116 float errorYaw[3];// = {0, 0, 0};
wisnup 0:2b4ec92bff8b 117 float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
wisnup 0:2b4ec92bff8b 118 float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
wisnup 0:2b4ec92bff8b 119 float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
wisnup 0:2b4ec92bff8b 120
wisnup 0:2b4ec92bff8b 121 // Euler angles
wisnup 0:2b4ec92bff8b 122 float yaw;
wisnup 0:2b4ec92bff8b 123 float pitch;
wisnup 0:2b4ec92bff8b 124 float roll;
wisnup 0:2b4ec92bff8b 125
wisnup 0:2b4ec92bff8b 126 float G_Dt; // Integration time for DCM algorithm
wisnup 0:2b4ec92bff8b 127
wisnup 0:2b4ec92bff8b 128 int rawMagnet[3];
wisnup 0:2b4ec92bff8b 129 int gyroBias[3];
wisnup 0:2b4ec92bff8b 130 };