Stéphane Legru / MinIMU_pops

Minimu 9v with an mbed, working on arduino program

Compass.h@0:fe2fd7f5dac3, 2013-11-17 (annotated)

Committer:
patsteph
Date:
Sun Nov 17 17:59:25 2013 +0000
Revision:
0:fe2fd7f5dac3
mbed working with arduino program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
patsteph 0:fe2fd7f5dac3 1 #include <L3GD20.h>
patsteph 0:fe2fd7f5dac3 2 #include <LSM303DLHC.h>
patsteph 0:fe2fd7f5dac3 3
patsteph 0:fe2fd7f5dac3 4 L3GD20 gyro(p28, p27);
patsteph 0:fe2fd7f5dac3 5 Serial debug(USBTX,USBRX);
patsteph 0:fe2fd7f5dac3 6 LSM303DLHC compass(p28, p27);
patsteph 0:fe2fd7f5dac3 7
patsteph 0:fe2fd7f5dac3 8 // Uncomment the below line to use this axis definition:
patsteph 0:fe2fd7f5dac3 9 // X axis pointing forward
patsteph 0:fe2fd7f5dac3 10 // Y axis pointing to the right
patsteph 0:fe2fd7f5dac3 11 // and Z axis pointing down.
patsteph 0:fe2fd7f5dac3 12 // Positive pitch : nose up
patsteph 0:fe2fd7f5dac3 13 // Positive roll : right wing down
patsteph 0:fe2fd7f5dac3 14 // Positive yaw : clockwise
patsteph 0:fe2fd7f5dac3 15 int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
patsteph 0:fe2fd7f5dac3 16 // Uncomment the below line to use this axis definition:
patsteph 0:fe2fd7f5dac3 17 // X axis pointing forward
patsteph 0:fe2fd7f5dac3 18 // Y axis pointing to the left
patsteph 0:fe2fd7f5dac3 19 // and Z axis pointing up.
patsteph 0:fe2fd7f5dac3 20 // Positive pitch : nose down
patsteph 0:fe2fd7f5dac3 21 // Positive roll : right wing down
patsteph 0:fe2fd7f5dac3 22 // Positive yaw : counterclockwise
patsteph 0:fe2fd7f5dac3 23 //int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
patsteph 0:fe2fd7f5dac3 24
patsteph 0:fe2fd7f5dac3 25 // tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168
patsteph 0:fe2fd7f5dac3 26
patsteph 0:fe2fd7f5dac3 27 // LSM303 accelerometer: 8 g sensitivity
patsteph 0:fe2fd7f5dac3 28 // 3.8 mg/digit; 1 g = 256
patsteph 0:fe2fd7f5dac3 29 #define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer
patsteph 0:fe2fd7f5dac3 30
patsteph 0:fe2fd7f5dac3 31 #define ToRad(x) ((x)*0.01745329252) // *pi/180
patsteph 0:fe2fd7f5dac3 32 #define ToDeg(x) ((x)*57.2957795131) // *180/pi
patsteph 0:fe2fd7f5dac3 33
patsteph 0:fe2fd7f5dac3 34 // L3G4200D gyro: 2000 dps full scale
patsteph 0:fe2fd7f5dac3 35 // 70 mdps/digit; 1 dps = 0.07
patsteph 0:fe2fd7f5dac3 36 #define Gyro_Gain_X 0.07 //X axis Gyro gain
patsteph 0:fe2fd7f5dac3 37 #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
patsteph 0:fe2fd7f5dac3 38 #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
patsteph 0:fe2fd7f5dac3 39 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
patsteph 0:fe2fd7f5dac3 40 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
patsteph 0:fe2fd7f5dac3 41 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
patsteph 0:fe2fd7f5dac3 42
patsteph 0:fe2fd7f5dac3 43 // LSM303 magnetometer calibration constants; use the Calibrate example from
patsteph 0:fe2fd7f5dac3 44 // the Pololu LSM303 library to find the right values for your board
patsteph 0:fe2fd7f5dac3 45 #define M_X_MIN -580
patsteph 0:fe2fd7f5dac3 46 #define M_Y_MIN -650
patsteph 0:fe2fd7f5dac3 47 #define M_Z_MIN -770
patsteph 0:fe2fd7f5dac3 48 #define M_X_MAX 495
patsteph 0:fe2fd7f5dac3 49 #define M_Y_MAX 480
patsteph 0:fe2fd7f5dac3 50 #define M_Z_MAX 370
patsteph 0:fe2fd7f5dac3 51 //MIN x: -576.000000 y: -645.000000 z: -767.000000
patsteph 0:fe2fd7f5dac3 52 //MAX x: 489.000000 y: 475.000000 z: 363.000000
patsteph 0:fe2fd7f5dac3 53
patsteph 0:fe2fd7f5dac3 54 #define Kp_ROLLPITCH 0.02
patsteph 0:fe2fd7f5dac3 55 #define Ki_ROLLPITCH 0.00002
patsteph 0:fe2fd7f5dac3 56 #define Kp_YAW 1.2
patsteph 0:fe2fd7f5dac3 57 #define Ki_YAW 0.00002
patsteph 0:fe2fd7f5dac3 58
patsteph 0:fe2fd7f5dac3 59 /*For debugging purposes*/
patsteph 0:fe2fd7f5dac3 60 //OUTPUTMODE=1 will print the corrected data,
patsteph 0:fe2fd7f5dac3 61 //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
patsteph 0:fe2fd7f5dac3 62 #define OUTPUTMODE 1
patsteph 0:fe2fd7f5dac3 63
patsteph 0:fe2fd7f5dac3 64 //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
patsteph 0:fe2fd7f5dac3 65 #define PRINT_ANALOGS 0 //Will print the analog raw data
patsteph 0:fe2fd7f5dac3 66 #define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw
patsteph 0:fe2fd7f5dac3 67
patsteph 0:fe2fd7f5dac3 68 #define STATUS_LED 13
patsteph 0:fe2fd7f5dac3 69
patsteph 0:fe2fd7f5dac3 70 float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
patsteph 0:fe2fd7f5dac3 71
patsteph 0:fe2fd7f5dac3 72 Timer tim; //general purpuse timer
patsteph 0:fe2fd7f5dac3 73 long timer=0;
patsteph 0:fe2fd7f5dac3 74 long timer_old;
patsteph 0:fe2fd7f5dac3 75 long timer24=0; //Second timer used to print values
patsteph 0:fe2fd7f5dac3 76 int AN[6]; //array that stores the gyro and accelerometer data
patsteph 0:fe2fd7f5dac3 77 int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
patsteph 0:fe2fd7f5dac3 78
patsteph 0:fe2fd7f5dac3 79 int gyro_x;
patsteph 0:fe2fd7f5dac3 80 int gyro_y;
patsteph 0:fe2fd7f5dac3 81 int gyro_z;
patsteph 0:fe2fd7f5dac3 82 int accel_x;
patsteph 0:fe2fd7f5dac3 83 int accel_y;
patsteph 0:fe2fd7f5dac3 84 int accel_z;
patsteph 0:fe2fd7f5dac3 85 int magnetom_x;
patsteph 0:fe2fd7f5dac3 86 int magnetom_y;
patsteph 0:fe2fd7f5dac3 87 int magnetom_z;
patsteph 0:fe2fd7f5dac3 88 float c_magnetom_x;
patsteph 0:fe2fd7f5dac3 89 float c_magnetom_y;
patsteph 0:fe2fd7f5dac3 90 float c_magnetom_z;
patsteph 0:fe2fd7f5dac3 91 float MAG_Heading;
patsteph 0:fe2fd7f5dac3 92
patsteph 0:fe2fd7f5dac3 93 float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
patsteph 0:fe2fd7f5dac3 94 float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
patsteph 0:fe2fd7f5dac3 95 float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
patsteph 0:fe2fd7f5dac3 96 float Omega_P[3]= {0,0,0};//Omega Proportional correction
patsteph 0:fe2fd7f5dac3 97 float Omega_I[3]= {0,0,0};//Omega Integrator
patsteph 0:fe2fd7f5dac3 98 float Omega[3]= {0,0,0};
patsteph 0:fe2fd7f5dac3 99
patsteph 0:fe2fd7f5dac3 100 // Euler angles
patsteph 0:fe2fd7f5dac3 101 float roll;
patsteph 0:fe2fd7f5dac3 102 float pitch;
patsteph 0:fe2fd7f5dac3 103 float yaw;
patsteph 0:fe2fd7f5dac3 104
patsteph 0:fe2fd7f5dac3 105 float errorRollPitch[3]= {0,0,0};
patsteph 0:fe2fd7f5dac3 106 float errorYaw[3]= {0,0,0};
patsteph 0:fe2fd7f5dac3 107
patsteph 0:fe2fd7f5dac3 108 unsigned int counter=0;
patsteph 0:fe2fd7f5dac3 109 int gyro_sat=0;
patsteph 0:fe2fd7f5dac3 110
patsteph 0:fe2fd7f5dac3 111 float DCM_Matrix[3][3]= {
patsteph 0:fe2fd7f5dac3 112 {
patsteph 0:fe2fd7f5dac3 113 1,0,0 }
patsteph 0:fe2fd7f5dac3 114 ,{
patsteph 0:fe2fd7f5dac3 115 0,1,0 }
patsteph 0:fe2fd7f5dac3 116 ,{
patsteph 0:fe2fd7f5dac3 117 0,0,1 }
patsteph 0:fe2fd7f5dac3 118 };
patsteph 0:fe2fd7f5dac3 119 float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
patsteph 0:fe2fd7f5dac3 120
patsteph 0:fe2fd7f5dac3 121
patsteph 0:fe2fd7f5dac3 122 float Temporary_Matrix[3][3]={
patsteph 0:fe2fd7f5dac3 123 {
patsteph 0:fe2fd7f5dac3 124 0,0,0 }
patsteph 0:fe2fd7f5dac3 125 ,{
patsteph 0:fe2fd7f5dac3 126 0,0,0 }
patsteph 0:fe2fd7f5dac3 127 ,{
patsteph 0:fe2fd7f5dac3 128 0,0,0 }
patsteph 0:fe2fd7f5dac3 129 };
patsteph 0:fe2fd7f5dac3 130
patsteph 0:fe2fd7f5dac3 131 float min(float x, float y)
patsteph 0:fe2fd7f5dac3 132 {
patsteph 0:fe2fd7f5dac3 133 if(x < y)
patsteph 0:fe2fd7f5dac3 134 {return x;}
patsteph 0:fe2fd7f5dac3 135 else
patsteph 0:fe2fd7f5dac3 136 {return y;}
patsteph 0:fe2fd7f5dac3 137 }
patsteph 0:fe2fd7f5dac3 138 float max(float x, float y)
patsteph 0:fe2fd7f5dac3 139 {
patsteph 0:fe2fd7f5dac3 140 if(x > y)
patsteph 0:fe2fd7f5dac3 141 {return x;}
patsteph 0:fe2fd7f5dac3 142 else
patsteph 0:fe2fd7f5dac3 143 {return y;}
patsteph 0:fe2fd7f5dac3 144 }
patsteph 0:fe2fd7f5dac3 145
patsteph 0:fe2fd7f5dac3 146 void Compass_Heading()
patsteph 0:fe2fd7f5dac3 147 {
patsteph 0:fe2fd7f5dac3 148 float MAG_X;
patsteph 0:fe2fd7f5dac3 149 float MAG_Y;
patsteph 0:fe2fd7f5dac3 150 float cos_roll;
patsteph 0:fe2fd7f5dac3 151 float sin_roll;
patsteph 0:fe2fd7f5dac3 152 float cos_pitch;
patsteph 0:fe2fd7f5dac3 153 float sin_pitch;
patsteph 0:fe2fd7f5dac3 154
patsteph 0:fe2fd7f5dac3 155 cos_roll = cos(roll);
patsteph 0:fe2fd7f5dac3 156 sin_roll = sin(roll);
patsteph 0:fe2fd7f5dac3 157 cos_pitch = cos(pitch);
patsteph 0:fe2fd7f5dac3 158 sin_pitch = sin(pitch);
patsteph 0:fe2fd7f5dac3 159
patsteph 0:fe2fd7f5dac3 160 // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
patsteph 0:fe2fd7f5dac3 161 c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5;
patsteph 0:fe2fd7f5dac3 162 c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5;
patsteph 0:fe2fd7f5dac3 163 c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5;
patsteph 0:fe2fd7f5dac3 164
patsteph 0:fe2fd7f5dac3 165 // Tilt compensated Magnetic filed X:
patsteph 0:fe2fd7f5dac3 166 MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
patsteph 0:fe2fd7f5dac3 167 // Tilt compensated Magnetic filed Y:
patsteph 0:fe2fd7f5dac3 168 MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
patsteph 0:fe2fd7f5dac3 169 // Magnetic Heading
patsteph 0:fe2fd7f5dac3 170 MAG_Heading = atan2(-MAG_Y,MAG_X);
patsteph 0:fe2fd7f5dac3 171 }
patsteph 0:fe2fd7f5dac3 172 float Vector_Dot_Product(float vector1[3],float vector2[3])
patsteph 0:fe2fd7f5dac3 173 {
patsteph 0:fe2fd7f5dac3 174 float op=0;
patsteph 0:fe2fd7f5dac3 175
patsteph 0:fe2fd7f5dac3 176 for(int c=0; c<3; c++)
patsteph 0:fe2fd7f5dac3 177 {
patsteph 0:fe2fd7f5dac3 178 op = op + (vector1[c]*vector2[c]);
patsteph 0:fe2fd7f5dac3 179 }
patsteph 0:fe2fd7f5dac3 180
patsteph 0:fe2fd7f5dac3 181 return op;
patsteph 0:fe2fd7f5dac3 182 }
patsteph 0:fe2fd7f5dac3 183 //Computes the cross product of two vectors
patsteph 0:fe2fd7f5dac3 184 void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
patsteph 0:fe2fd7f5dac3 185 {
patsteph 0:fe2fd7f5dac3 186 vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
patsteph 0:fe2fd7f5dac3 187 vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
patsteph 0:fe2fd7f5dac3 188 vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
patsteph 0:fe2fd7f5dac3 189 }
patsteph 0:fe2fd7f5dac3 190
patsteph 0:fe2fd7f5dac3 191 //Multiply the vector by a scalar.
patsteph 0:fe2fd7f5dac3 192 void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
patsteph 0:fe2fd7f5dac3 193 {
patsteph 0:fe2fd7f5dac3 194 for(int c=0; c<3; c++)
patsteph 0:fe2fd7f5dac3 195 {
patsteph 0:fe2fd7f5dac3 196 vectorOut[c]=vectorIn[c]*scale2;
patsteph 0:fe2fd7f5dac3 197 }
patsteph 0:fe2fd7f5dac3 198 }
patsteph 0:fe2fd7f5dac3 199
patsteph 0:fe2fd7f5dac3 200 void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
patsteph 0:fe2fd7f5dac3 201 {
patsteph 0:fe2fd7f5dac3 202 for(int c=0; c<3; c++)
patsteph 0:fe2fd7f5dac3 203 {
patsteph 0:fe2fd7f5dac3 204 vectorOut[c]=vectorIn1[c]+vectorIn2[c];
patsteph 0:fe2fd7f5dac3 205 }
patsteph 0:fe2fd7f5dac3 206 }
patsteph 0:fe2fd7f5dac3 207 /**************************************************/
patsteph 0:fe2fd7f5dac3 208 void Normalize(void)
patsteph 0:fe2fd7f5dac3 209 {
patsteph 0:fe2fd7f5dac3 210 float error=0;
patsteph 0:fe2fd7f5dac3 211 float temporary[3][3];
patsteph 0:fe2fd7f5dac3 212 float renorm=0;
patsteph 0:fe2fd7f5dac3 213
patsteph 0:fe2fd7f5dac3 214 error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
patsteph 0:fe2fd7f5dac3 215
patsteph 0:fe2fd7f5dac3 216 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
patsteph 0:fe2fd7f5dac3 217 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
patsteph 0:fe2fd7f5dac3 218
patsteph 0:fe2fd7f5dac3 219 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
patsteph 0:fe2fd7f5dac3 220 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
patsteph 0:fe2fd7f5dac3 221
patsteph 0:fe2fd7f5dac3 222 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
patsteph 0:fe2fd7f5dac3 223
patsteph 0:fe2fd7f5dac3 224 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
patsteph 0:fe2fd7f5dac3 225 Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
patsteph 0:fe2fd7f5dac3 226
patsteph 0:fe2fd7f5dac3 227 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
patsteph 0:fe2fd7f5dac3 228 Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
patsteph 0:fe2fd7f5dac3 229
patsteph 0:fe2fd7f5dac3 230 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
patsteph 0:fe2fd7f5dac3 231 Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
patsteph 0:fe2fd7f5dac3 232 }
patsteph 0:fe2fd7f5dac3 233
patsteph 0:fe2fd7f5dac3 234 float constrain(float x,float a, float b)
patsteph 0:fe2fd7f5dac3 235 {
patsteph 0:fe2fd7f5dac3 236 if(x <a)
patsteph 0:fe2fd7f5dac3 237 {
patsteph 0:fe2fd7f5dac3 238 return a;
patsteph 0:fe2fd7f5dac3 239 }
patsteph 0:fe2fd7f5dac3 240 if(x > b)
patsteph 0:fe2fd7f5dac3 241 {
patsteph 0:fe2fd7f5dac3 242 return b;
patsteph 0:fe2fd7f5dac3 243 }
patsteph 0:fe2fd7f5dac3 244 return x;
patsteph 0:fe2fd7f5dac3 245 }
patsteph 0:fe2fd7f5dac3 246 /**************************************************/
patsteph 0:fe2fd7f5dac3 247 void Drift_correction(void)
patsteph 0:fe2fd7f5dac3 248 {
patsteph 0:fe2fd7f5dac3 249 float mag_heading_x;
patsteph 0:fe2fd7f5dac3 250 float mag_heading_y;
patsteph 0:fe2fd7f5dac3 251 float errorCourse;
patsteph 0:fe2fd7f5dac3 252 //Compensation the Roll, Pitch and Yaw drift.
patsteph 0:fe2fd7f5dac3 253 static float Scaled_Omega_P[3];
patsteph 0:fe2fd7f5dac3 254 static float Scaled_Omega_I[3];
patsteph 0:fe2fd7f5dac3 255 float Accel_magnitude;
patsteph 0:fe2fd7f5dac3 256 float Accel_weight;
patsteph 0:fe2fd7f5dac3 257
patsteph 0:fe2fd7f5dac3 258
patsteph 0:fe2fd7f5dac3 259 //*****Roll and Pitch***************
patsteph 0:fe2fd7f5dac3 260
patsteph 0:fe2fd7f5dac3 261 // Calculate the magnitude of the accelerometer vector
patsteph 0:fe2fd7f5dac3 262 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
patsteph 0:fe2fd7f5dac3 263 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
patsteph 0:fe2fd7f5dac3 264 // Dynamic weighting of accelerometer info (reliability filter)
patsteph 0:fe2fd7f5dac3 265 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
patsteph 0:fe2fd7f5dac3 266 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
patsteph 0:fe2fd7f5dac3 267
patsteph 0:fe2fd7f5dac3 268 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
patsteph 0:fe2fd7f5dac3 269 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
patsteph 0:fe2fd7f5dac3 270
patsteph 0:fe2fd7f5dac3 271 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
patsteph 0:fe2fd7f5dac3 272 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
patsteph 0:fe2fd7f5dac3 273
patsteph 0:fe2fd7f5dac3 274 //*****YAW***************
patsteph 0:fe2fd7f5dac3 275 // We make the gyro YAW drift correction based on compass magnetic heading
patsteph 0:fe2fd7f5dac3 276
patsteph 0:fe2fd7f5dac3 277 mag_heading_x = cos(MAG_Heading);
patsteph 0:fe2fd7f5dac3 278 mag_heading_y = sin(MAG_Heading);
patsteph 0:fe2fd7f5dac3 279 errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
patsteph 0:fe2fd7f5dac3 280 Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
patsteph 0:fe2fd7f5dac3 281
patsteph 0:fe2fd7f5dac3 282 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
patsteph 0:fe2fd7f5dac3 283 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
patsteph 0:fe2fd7f5dac3 284
patsteph 0:fe2fd7f5dac3 285 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
patsteph 0:fe2fd7f5dac3 286 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
patsteph 0:fe2fd7f5dac3 287 }
patsteph 0:fe2fd7f5dac3 288 /**************************************************/
patsteph 0:fe2fd7f5dac3 289 /*
patsteph 0:fe2fd7f5dac3 290 void Accel_adjust(void)
patsteph 0:fe2fd7f5dac3 291 {
patsteph 0:fe2fd7f5dac3 292 Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
patsteph 0:fe2fd7f5dac3 293 Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
patsteph 0:fe2fd7f5dac3 294 }
patsteph 0:fe2fd7f5dac3 295 */
patsteph 0:fe2fd7f5dac3 296 /**************************************************/
patsteph 0:fe2fd7f5dac3 297 /**************************************************/
patsteph 0:fe2fd7f5dac3 298 //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
patsteph 0:fe2fd7f5dac3 299 void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
patsteph 0:fe2fd7f5dac3 300 {
patsteph 0:fe2fd7f5dac3 301 float op[3];
patsteph 0:fe2fd7f5dac3 302 for(int x=0; x<3; x++)
patsteph 0:fe2fd7f5dac3 303 {
patsteph 0:fe2fd7f5dac3 304 for(int y=0; y<3; y++)
patsteph 0:fe2fd7f5dac3 305 {
patsteph 0:fe2fd7f5dac3 306 for(int w=0; w<3; w++)
patsteph 0:fe2fd7f5dac3 307 {
patsteph 0:fe2fd7f5dac3 308 op[w]=a[x][w]*b[w][y];
patsteph 0:fe2fd7f5dac3 309 }
patsteph 0:fe2fd7f5dac3 310 mat[x][y]=0;
patsteph 0:fe2fd7f5dac3 311 mat[x][y]=op[0]+op[1]+op[2];
patsteph 0:fe2fd7f5dac3 312 }
patsteph 0:fe2fd7f5dac3 313 }
patsteph 0:fe2fd7f5dac3 314 }
patsteph 0:fe2fd7f5dac3 315 void Matrix_update(void)
patsteph 0:fe2fd7f5dac3 316 {
patsteph 0:fe2fd7f5dac3 317 Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
patsteph 0:fe2fd7f5dac3 318 Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
patsteph 0:fe2fd7f5dac3 319 Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
patsteph 0:fe2fd7f5dac3 320
patsteph 0:fe2fd7f5dac3 321 Accel_Vector[0]=accel_x;
patsteph 0:fe2fd7f5dac3 322 Accel_Vector[1]=accel_y;
patsteph 0:fe2fd7f5dac3 323 Accel_Vector[2]=accel_z;
patsteph 0:fe2fd7f5dac3 324
patsteph 0:fe2fd7f5dac3 325 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_P[0]); //adding proportional term
patsteph 0:fe2fd7f5dac3 326 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_I[0]); //adding Integrator term
patsteph 0:fe2fd7f5dac3 327
patsteph 0:fe2fd7f5dac3 328 //Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
patsteph 0:fe2fd7f5dac3 329
patsteph 0:fe2fd7f5dac3 330 Update_Matrix[0][0]=0;
patsteph 0:fe2fd7f5dac3 331 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
patsteph 0:fe2fd7f5dac3 332 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
patsteph 0:fe2fd7f5dac3 333 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
patsteph 0:fe2fd7f5dac3 334 Update_Matrix[1][1]=0;
patsteph 0:fe2fd7f5dac3 335 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
patsteph 0:fe2fd7f5dac3 336 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
patsteph 0:fe2fd7f5dac3 337 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
patsteph 0:fe2fd7f5dac3 338 Update_Matrix[2][2]=0;
patsteph 0:fe2fd7f5dac3 339
patsteph 0:fe2fd7f5dac3 340 Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
patsteph 0:fe2fd7f5dac3 341
patsteph 0:fe2fd7f5dac3 342 for(int x=0; x<3; x++) //Matrix Addition (update)
patsteph 0:fe2fd7f5dac3 343 {
patsteph 0:fe2fd7f5dac3 344 for(int y=0; y<3; y++)
patsteph 0:fe2fd7f5dac3 345 {
patsteph 0:fe2fd7f5dac3 346 DCM_Matrix[x][y] = DCM_Matrix[x][y]+ Temporary_Matrix[x][y];
patsteph 0:fe2fd7f5dac3 347 }
patsteph 0:fe2fd7f5dac3 348 }
patsteph 0:fe2fd7f5dac3 349 }
patsteph 0:fe2fd7f5dac3 350
patsteph 0:fe2fd7f5dac3 351 void Euler_angles(void)
patsteph 0:fe2fd7f5dac3 352 {
patsteph 0:fe2fd7f5dac3 353 pitch = -asin(DCM_Matrix[2][0]);
patsteph 0:fe2fd7f5dac3 354 roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
patsteph 0:fe2fd7f5dac3 355 yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
patsteph 0:fe2fd7f5dac3 356 }
patsteph 0:fe2fd7f5dac3 357
patsteph 0:fe2fd7f5dac3 358
patsteph 0:fe2fd7f5dac3 359 void Gyro_Init()
patsteph 0:fe2fd7f5dac3 360 {
patsteph 0:fe2fd7f5dac3 361 //gyro.init(); OK
patsteph 0:fe2fd7f5dac3 362 char data = 0x0F;
patsteph 0:fe2fd7f5dac3 363 gyro.write_reg(GYR_ADDRESS, L3GD20_CTRL_REG1, data); // normal power mode, all axes enabled, 100 Hz
patsteph 0:fe2fd7f5dac3 364 data = 0x20;
patsteph 0:fe2fd7f5dac3 365 gyro.write_reg(GYR_ADDRESS, L3GD20_CTRL_REG4, data); // 2000 dps full scale
patsteph 0:fe2fd7f5dac3 366 }
patsteph 0:fe2fd7f5dac3 367
patsteph 0:fe2fd7f5dac3 368 void Read_Gyro(float* gx, float* gy, float* gz)
patsteph 0:fe2fd7f5dac3 369 {
patsteph 0:fe2fd7f5dac3 370 gyro.read(gx, gy, gz);
patsteph 0:fe2fd7f5dac3 371 AN[0] = *gx;
patsteph 0:fe2fd7f5dac3 372 AN[1] = *gy;
patsteph 0:fe2fd7f5dac3 373 AN[2] = *gz;
patsteph 0:fe2fd7f5dac3 374 gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
patsteph 0:fe2fd7f5dac3 375 gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
patsteph 0:fe2fd7f5dac3 376 gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
patsteph 0:fe2fd7f5dac3 377 }
patsteph 0:fe2fd7f5dac3 378
patsteph 0:fe2fd7f5dac3 379 void Accel_Init()
patsteph 0:fe2fd7f5dac3 380 {
patsteph 0:fe2fd7f5dac3 381 // compass.init(); OK
patsteph 0:fe2fd7f5dac3 382 char data = 0x47;
patsteph 0:fe2fd7f5dac3 383 compass.write_reg(0x32, CTRL_REG1_A, data); // normal power mode, all axes enabled, 50 Hz ACC
patsteph 0:fe2fd7f5dac3 384 data = 0x28;
patsteph 0:fe2fd7f5dac3 385 compass.write_reg(0x32, CTRL_REG4_A, data); // 8 g full scale: FS = 10 on DLHC; high resolution output mode ACC
patsteph 0:fe2fd7f5dac3 386 }
patsteph 0:fe2fd7f5dac3 387
patsteph 0:fe2fd7f5dac3 388 // Reads x,y and z accelerometer registers
patsteph 0:fe2fd7f5dac3 389 void Read_Accel(float* ax, float* ay, float* az)
patsteph 0:fe2fd7f5dac3 390 {
patsteph 0:fe2fd7f5dac3 391 compass.readacc(ax, ay, az);
patsteph 0:fe2fd7f5dac3 392
patsteph 0:fe2fd7f5dac3 393 AN[3] = *ax;
patsteph 0:fe2fd7f5dac3 394 AN[4] = *ay;
patsteph 0:fe2fd7f5dac3 395 AN[5] = *az;
patsteph 0:fe2fd7f5dac3 396 accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
patsteph 0:fe2fd7f5dac3 397 accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
patsteph 0:fe2fd7f5dac3 398 accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
patsteph 0:fe2fd7f5dac3 399 }
patsteph 0:fe2fd7f5dac3 400
patsteph 0:fe2fd7f5dac3 401 void Compass_Init()
patsteph 0:fe2fd7f5dac3 402 {
patsteph 0:fe2fd7f5dac3 403 char data = 0x00;
patsteph 0:fe2fd7f5dac3 404 compass.write_reg(0x3c, CTRL_REG1_A, data); // continuous conversion mode
patsteph 0:fe2fd7f5dac3 405 // 15 Hz default
patsteph 0:fe2fd7f5dac3 406 }
patsteph 0:fe2fd7f5dac3 407
patsteph 0:fe2fd7f5dac3 408 void Read_Compass(float* mx, float* my, float* mz)
patsteph 0:fe2fd7f5dac3 409 {
patsteph 0:fe2fd7f5dac3 410 compass.readcomp(mx, my, mz);
patsteph 0:fe2fd7f5dac3 411
patsteph 0:fe2fd7f5dac3 412 magnetom_x = SENSOR_SIGN[6] * (*mx);
patsteph 0:fe2fd7f5dac3 413 magnetom_y = SENSOR_SIGN[7] * (*my);
patsteph 0:fe2fd7f5dac3 414 magnetom_z = SENSOR_SIGN[8] * (*mz);
patsteph 0:fe2fd7f5dac3 415 }
patsteph 0:fe2fd7f5dac3 416
patsteph 0:fe2fd7f5dac3 417 void printdata(Serial debug)
patsteph 0:fe2fd7f5dac3 418 {
patsteph 0:fe2fd7f5dac3 419 debug.printf("!");
patsteph 0:fe2fd7f5dac3 420 debug.printf("ANG:");
patsteph 0:fe2fd7f5dac3 421 debug.printf("%lf",ToDeg(roll));
patsteph 0:fe2fd7f5dac3 422 debug.printf(",");
patsteph 0:fe2fd7f5dac3 423 debug.printf("%lf",ToDeg(pitch));
patsteph 0:fe2fd7f5dac3 424 debug.printf(",");
patsteph 0:fe2fd7f5dac3 425 debug.printf("%lf",ToDeg(yaw));
patsteph 0:fe2fd7f5dac3 426 debug.printf("\n\r");
patsteph 0:fe2fd7f5dac3 427
patsteph 0:fe2fd7f5dac3 428 }
patsteph 0:fe2fd7f5dac3 429
patsteph 0:fe2fd7f5dac3 430 long convert_to_dec(float x)
patsteph 0:fe2fd7f5dac3 431 {
patsteph 0:fe2fd7f5dac3 432 return x*10000000;
patsteph 0:fe2fd7f5dac3 433 }
patsteph 0:fe2fd7f5dac3 434 //Computes the dot product of two vectors
patsteph 0:fe2fd7f5dac3 435
patsteph 0:fe2fd7f5dac3 436