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?
User | Revision | Line number | New 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 |