AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
minimu9.cpp@0:dc35364e2291, 2012-04-12 (annotated)
- Committer:
- krmreynolds
- Date:
- Thu Apr 12 13:47:23 2012 +0000
- Revision:
- 0:dc35364e2291
- Child:
- 1:3272ece36ce1
Added GNU Agreement
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krmreynolds | 0:dc35364e2291 | 1 | /* mbed L3G4200D Library version 0.1 |
krmreynolds | 0:dc35364e2291 | 2 | * Copyright (c) 2012 Prediluted |
krmreynolds | 0:dc35364e2291 | 3 | * |
krmreynolds | 0:dc35364e2291 | 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
krmreynolds | 0:dc35364e2291 | 5 | * of this software and associated documentation files (the "Software"), to deal |
krmreynolds | 0:dc35364e2291 | 6 | * in the Software without restriction, including without limitation the rights |
krmreynolds | 0:dc35364e2291 | 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
krmreynolds | 0:dc35364e2291 | 8 | * copies of the Software, and to permit persons to whom the Software is |
krmreynolds | 0:dc35364e2291 | 9 | * furnished to do so, subject to the following conditions: |
krmreynolds | 0:dc35364e2291 | 10 | * |
krmreynolds | 0:dc35364e2291 | 11 | * The above copyright notice and this permission notice shall be included in |
krmreynolds | 0:dc35364e2291 | 12 | * all copies or substantial portions of the Software. |
krmreynolds | 0:dc35364e2291 | 13 | * |
krmreynolds | 0:dc35364e2291 | 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
krmreynolds | 0:dc35364e2291 | 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
krmreynolds | 0:dc35364e2291 | 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
krmreynolds | 0:dc35364e2291 | 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
krmreynolds | 0:dc35364e2291 | 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
krmreynolds | 0:dc35364e2291 | 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
krmreynolds | 0:dc35364e2291 | 20 | * THE SOFTWARE. |
krmreynolds | 0:dc35364e2291 | 21 | */ |
krmreynolds | 0:dc35364e2291 | 22 | |
krmreynolds | 0:dc35364e2291 | 23 | /* |
krmreynolds | 0:dc35364e2291 | 24 | * Communication for the minimu9, ported from the original Polulu minIMU-9 Arduino code |
krmreynolds | 0:dc35364e2291 | 25 | */ |
krmreynolds | 0:dc35364e2291 | 26 | #include <stdlib.h> |
krmreynolds | 0:dc35364e2291 | 27 | #include <algorithm> |
krmreynolds | 0:dc35364e2291 | 28 | #include <iostream> |
krmreynolds | 0:dc35364e2291 | 29 | #include <vector> |
krmreynolds | 0:dc35364e2291 | 30 | #include <math.h> |
krmreynolds | 0:dc35364e2291 | 31 | #include "minimu9.h" |
krmreynolds | 0:dc35364e2291 | 32 | #include "L3G4200D.h" |
krmreynolds | 0:dc35364e2291 | 33 | #include "LSM303.h" |
krmreynolds | 0:dc35364e2291 | 34 | #include "Matrix.h" |
krmreynolds | 0:dc35364e2291 | 35 | |
krmreynolds | 0:dc35364e2291 | 36 | |
krmreynolds | 0:dc35364e2291 | 37 | /* |
krmreynolds | 0:dc35364e2291 | 38 | * void setup() replaced with the constructor |
krmreynolds | 0:dc35364e2291 | 39 | */ |
krmreynolds | 0:dc35364e2291 | 40 | minimu9::minimu9() { |
krmreynolds | 0:dc35364e2291 | 41 | // Calibration constants defaults, will need to calibrate then use |
krmreynolds | 0:dc35364e2291 | 42 | // set_calibration_constants() to have the proper values for your board |
krmreynolds | 0:dc35364e2291 | 43 | M_X_MIN = -570; |
krmreynolds | 0:dc35364e2291 | 44 | M_Y_MIN = -746; |
krmreynolds | 0:dc35364e2291 | 45 | M_Z_MIN = -416; |
krmreynolds | 0:dc35364e2291 | 46 | M_X_MAX = 477; |
krmreynolds | 0:dc35364e2291 | 47 | M_Y_MAX = 324; |
krmreynolds | 0:dc35364e2291 | 48 | M_Z_MAX = 652; |
krmreynolds | 0:dc35364e2291 | 49 | |
krmreynolds | 0:dc35364e2291 | 50 | /*For debugging purposes*/ |
krmreynolds | 0:dc35364e2291 | 51 | //OUTPUTMODE=1 will print the corrected data, |
krmreynolds | 0:dc35364e2291 | 52 | //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) |
krmreynolds | 0:dc35364e2291 | 53 | OUTPUTMODE = 1; |
krmreynolds | 0:dc35364e2291 | 54 | |
krmreynolds | 0:dc35364e2291 | 55 | //#define PRINT_DCM 0 //Will print the whole direction cosine matrix |
krmreynolds | 0:dc35364e2291 | 56 | PRINT_ANALOGS = 0; //Will print the analog raw data |
krmreynolds | 0:dc35364e2291 | 57 | PRINT_EULER = 1; //Will print the Euler angles Roll, Pitch and Yaw |
krmreynolds | 0:dc35364e2291 | 58 | PRINT_DCM = 0; |
krmreynolds | 0:dc35364e2291 | 59 | |
krmreynolds | 0:dc35364e2291 | 60 | PRINT_SERIAL = 0; |
krmreynolds | 0:dc35364e2291 | 61 | //PRINT_SERIAL = 1; |
krmreynolds | 0:dc35364e2291 | 62 | |
krmreynolds | 0:dc35364e2291 | 63 | t.start(); |
krmreynolds | 0:dc35364e2291 | 64 | #if PRINT_SERIAL == 1 |
krmreynolds | 0:dc35364e2291 | 65 | Serial serial(USBTX, USBRX); |
krmreynolds | 0:dc35364e2291 | 66 | #endif |
krmreynolds | 0:dc35364e2291 | 67 | gyro = new L3G4200D( p9, p10 ); |
krmreynolds | 0:dc35364e2291 | 68 | compass = new LSM303( p9, p10 ); |
krmreynolds | 0:dc35364e2291 | 69 | I2C i2c(p9, p10); |
krmreynolds | 0:dc35364e2291 | 70 | |
krmreynolds | 0:dc35364e2291 | 71 | wait(1.5); |
krmreynolds | 0:dc35364e2291 | 72 | /* |
krmreynolds | 0:dc35364e2291 | 73 | * Give variables initial values |
krmreynolds | 0:dc35364e2291 | 74 | */ |
krmreynolds | 0:dc35364e2291 | 75 | G_Dt=0.02; |
krmreynolds | 0:dc35364e2291 | 76 | timer=0; |
krmreynolds | 0:dc35364e2291 | 77 | timer24=0; |
krmreynolds | 0:dc35364e2291 | 78 | counter=0; |
krmreynolds | 0:dc35364e2291 | 79 | gyro_sat=0; |
krmreynolds | 0:dc35364e2291 | 80 | |
krmreynolds | 0:dc35364e2291 | 81 | memcpy(SENSOR_SIGN, (int [9]) { |
krmreynolds | 0:dc35364e2291 | 82 | 1,1,1,-1,-1,-1,1,1,1 |
krmreynolds | 0:dc35364e2291 | 83 | }, 9*sizeof(int)); |
krmreynolds | 0:dc35364e2291 | 84 | memcpy(AN_OFFSET, (int [6]) { |
krmreynolds | 0:dc35364e2291 | 85 | 0,0,0,0,0,0 |
krmreynolds | 0:dc35364e2291 | 86 | }, 6*sizeof(int)); |
krmreynolds | 0:dc35364e2291 | 87 | memcpy(Accel_Vector, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 88 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 89 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 90 | memcpy(Gyro_Vector, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 91 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 92 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 93 | memcpy(Omega_Vector, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 94 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 95 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 96 | memcpy(Omega_P, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 97 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 98 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 99 | memcpy(Omega_I, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 100 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 101 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 102 | memcpy(Omega, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 103 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 104 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 105 | memcpy(errorRollPitch, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 106 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 107 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 108 | memcpy(errorYaw, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 109 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 110 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 111 | memcpy( DCM_Matrix, (float[3][3]) { |
krmreynolds | 0:dc35364e2291 | 112 | { |
krmreynolds | 0:dc35364e2291 | 113 | 1,0,0 |
krmreynolds | 0:dc35364e2291 | 114 | }, { 0,1,0 }, { 0,0,1 } |
krmreynolds | 0:dc35364e2291 | 115 | }, 9*sizeof(float) ); |
krmreynolds | 0:dc35364e2291 | 116 | memcpy( Update_Matrix, (float[3][3]) { |
krmreynolds | 0:dc35364e2291 | 117 | { |
krmreynolds | 0:dc35364e2291 | 118 | 0,1,2 |
krmreynolds | 0:dc35364e2291 | 119 | }, { 3,4,5 }, { 6,7,8 } |
krmreynolds | 0:dc35364e2291 | 120 | }, 9*sizeof(float) ); |
krmreynolds | 0:dc35364e2291 | 121 | memcpy( Temporary_Matrix, (float[3][3]) { |
krmreynolds | 0:dc35364e2291 | 122 | { |
krmreynolds | 0:dc35364e2291 | 123 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 124 | }, { 0,0,0 }, { 0,0,0 } |
krmreynolds | 0:dc35364e2291 | 125 | }, 9*sizeof(float) ); |
krmreynolds | 0:dc35364e2291 | 126 | |
krmreynolds | 0:dc35364e2291 | 127 | /* |
krmreynolds | 0:dc35364e2291 | 128 | * Initialize the accel, compass, and gyro |
krmreynolds | 0:dc35364e2291 | 129 | */ |
krmreynolds | 0:dc35364e2291 | 130 | // Accel |
krmreynolds | 0:dc35364e2291 | 131 | compass->accRegisterWrite(LSM303::ACC_CTRL_REG1, 0x24); // normal power mode, all axes enabled, 50 Hz |
krmreynolds | 0:dc35364e2291 | 132 | compass->accRegisterWrite(LSM303::ACC_CTRL_REG4, 0x30 ); // 8g full scale |
krmreynolds | 0:dc35364e2291 | 133 | // Compass |
krmreynolds | 0:dc35364e2291 | 134 | compass->magRegisterWrite(LSM303::MAG_MR_REG, 0x00); // continuous conversion mode |
krmreynolds | 0:dc35364e2291 | 135 | // Gyro |
krmreynolds | 0:dc35364e2291 | 136 | gyro->registerWrite(L3G4200D::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz |
krmreynolds | 0:dc35364e2291 | 137 | gyro->registerWrite(L3G4200D::CTRL_REG4, 0x20); // 2000 dps full scale |
krmreynolds | 0:dc35364e2291 | 138 | |
krmreynolds | 0:dc35364e2291 | 139 | #if PRINT_SERIAL == 1 |
krmreynolds | 0:dc35364e2291 | 140 | serial.printf( "initiatied\n" ); |
krmreynolds | 0:dc35364e2291 | 141 | } |
krmreynolds | 0:dc35364e2291 | 142 | #endif |
krmreynolds | 0:dc35364e2291 | 143 | |
krmreynolds | 0:dc35364e2291 | 144 | for (int i=0; i<32; i++) { // We take some readings... |
krmreynolds | 0:dc35364e2291 | 145 | Read_Gyro(); |
krmreynolds | 0:dc35364e2291 | 146 | Read_Accel(); |
krmreynolds | 0:dc35364e2291 | 147 | for (int y=0; y<6; y++) // Cumulate values |
krmreynolds | 0:dc35364e2291 | 148 | AN_OFFSET[y] += AN[y]; |
krmreynolds | 0:dc35364e2291 | 149 | wait(0.2); |
krmreynolds | 0:dc35364e2291 | 150 | } |
krmreynolds | 0:dc35364e2291 | 151 | |
krmreynolds | 0:dc35364e2291 | 152 | for (int y=0; y<6; y++) |
krmreynolds | 0:dc35364e2291 | 153 | AN_OFFSET[y] = AN_OFFSET[y]/32; |
krmreynolds | 0:dc35364e2291 | 154 | |
krmreynolds | 0:dc35364e2291 | 155 | AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; |
krmreynolds | 0:dc35364e2291 | 156 | |
krmreynolds | 0:dc35364e2291 | 157 | #if PRINT_SERIAL == 1 |
krmreynolds | 0:dc35364e2291 | 158 | serial.printf("Offset:\n"); |
krmreynolds | 0:dc35364e2291 | 159 | for (int y=0; y<6; y++) { |
krmreynolds | 0:dc35364e2291 | 160 | serial.printf( "%d", AN_OFFSET[y] ); |
krmreynolds | 0:dc35364e2291 | 161 | } |
krmreynolds | 0:dc35364e2291 | 162 | serial.printf( "\n" ); |
krmreynolds | 0:dc35364e2291 | 163 | #endif |
krmreynolds | 0:dc35364e2291 | 164 | wait(2); |
krmreynolds | 0:dc35364e2291 | 165 | timer=t.read_ms(); |
krmreynolds | 0:dc35364e2291 | 166 | wait(0.2); |
krmreynolds | 0:dc35364e2291 | 167 | counter=0; |
krmreynolds | 0:dc35364e2291 | 168 | |
krmreynolds | 0:dc35364e2291 | 169 | #if PRINT_SERIAL == 1 |
krmreynolds | 0:dc35364e2291 | 170 | while ( 1 ) { |
krmreynolds | 0:dc35364e2291 | 171 | get_data(); |
krmreynolds | 0:dc35364e2291 | 172 | } |
krmreynolds | 0:dc35364e2291 | 173 | #endif |
krmreynolds | 0:dc35364e2291 | 174 | } |
krmreynolds | 0:dc35364e2291 | 175 | |
krmreynolds | 0:dc35364e2291 | 176 | void minimu9::get_data() { //Main Loop |
krmreynolds | 0:dc35364e2291 | 177 | if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz |
krmreynolds | 0:dc35364e2291 | 178 | counter++; |
krmreynolds | 0:dc35364e2291 | 179 | timer_old = timer; |
krmreynolds | 0:dc35364e2291 | 180 | timer=t.read_ms(); |
krmreynolds | 0:dc35364e2291 | 181 | if (timer>timer_old) |
krmreynolds | 0:dc35364e2291 | 182 | G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) |
krmreynolds | 0:dc35364e2291 | 183 | else |
krmreynolds | 0:dc35364e2291 | 184 | G_Dt = 0; |
krmreynolds | 0:dc35364e2291 | 185 | |
krmreynolds | 0:dc35364e2291 | 186 | // *** DCM algorithm |
krmreynolds | 0:dc35364e2291 | 187 | // Data adquisition |
krmreynolds | 0:dc35364e2291 | 188 | Read_Gyro(); // This read gyro data |
krmreynolds | 0:dc35364e2291 | 189 | Read_Accel(); // Read I2C accelerometer |
krmreynolds | 0:dc35364e2291 | 190 | |
krmreynolds | 0:dc35364e2291 | 191 | if (counter > 5) { // Read compass data at 10Hz... (5 loop runs) |
krmreynolds | 0:dc35364e2291 | 192 | counter=0; |
krmreynolds | 0:dc35364e2291 | 193 | Read_Compass(); // Read I2C magnetometer |
krmreynolds | 0:dc35364e2291 | 194 | Compass_Heading(); // Calculate magnetic heading |
krmreynolds | 0:dc35364e2291 | 195 | } |
krmreynolds | 0:dc35364e2291 | 196 | |
krmreynolds | 0:dc35364e2291 | 197 | // Calculations... |
krmreynolds | 0:dc35364e2291 | 198 | Matrix_update(); |
krmreynolds | 0:dc35364e2291 | 199 | Normalize(); |
krmreynolds | 0:dc35364e2291 | 200 | Drift_correction(); |
krmreynolds | 0:dc35364e2291 | 201 | Euler_angles(); |
krmreynolds | 0:dc35364e2291 | 202 | // *** |
krmreynolds | 0:dc35364e2291 | 203 | |
krmreynolds | 0:dc35364e2291 | 204 | if ( 1 == PRINT_SERIAL ) { |
krmreynolds | 0:dc35364e2291 | 205 | print_data(); |
krmreynolds | 0:dc35364e2291 | 206 | } |
krmreynolds | 0:dc35364e2291 | 207 | } |
krmreynolds | 0:dc35364e2291 | 208 | |
krmreynolds | 0:dc35364e2291 | 209 | } |
krmreynolds | 0:dc35364e2291 | 210 | |
krmreynolds | 0:dc35364e2291 | 211 | long convert_to_dec(float x) { |
krmreynolds | 0:dc35364e2291 | 212 | return x*10000000; |
krmreynolds | 0:dc35364e2291 | 213 | } |
krmreynolds | 0:dc35364e2291 | 214 | |
krmreynolds | 0:dc35364e2291 | 215 | void minimu9::Read_Gyro() { |
krmreynolds | 0:dc35364e2291 | 216 | gyro->read(); |
krmreynolds | 0:dc35364e2291 | 217 | std::vector<short> g = gyro->read(); |
krmreynolds | 0:dc35364e2291 | 218 | AN[0] = g[0]; |
krmreynolds | 0:dc35364e2291 | 219 | AN[1] = g[1]; |
krmreynolds | 0:dc35364e2291 | 220 | AN[2] = g[2]; |
krmreynolds | 0:dc35364e2291 | 221 | |
krmreynolds | 0:dc35364e2291 | 222 | gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]); |
krmreynolds | 0:dc35364e2291 | 223 | gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]); |
krmreynolds | 0:dc35364e2291 | 224 | gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]); |
krmreynolds | 0:dc35364e2291 | 225 | } |
krmreynolds | 0:dc35364e2291 | 226 | |
krmreynolds | 0:dc35364e2291 | 227 | |
krmreynolds | 0:dc35364e2291 | 228 | // Reads x,y and z accelerometer registers |
krmreynolds | 0:dc35364e2291 | 229 | void minimu9::Read_Accel() { |
krmreynolds | 0:dc35364e2291 | 230 | std::vector<short> a = compass->accRead(); |
krmreynolds | 0:dc35364e2291 | 231 | |
krmreynolds | 0:dc35364e2291 | 232 | AN[3] = a[0]; |
krmreynolds | 0:dc35364e2291 | 233 | AN[4] = a[1]; |
krmreynolds | 0:dc35364e2291 | 234 | AN[5] = a[2]; |
krmreynolds | 0:dc35364e2291 | 235 | |
krmreynolds | 0:dc35364e2291 | 236 | accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]); |
krmreynolds | 0:dc35364e2291 | 237 | accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]); |
krmreynolds | 0:dc35364e2291 | 238 | accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]); |
krmreynolds | 0:dc35364e2291 | 239 | } |
krmreynolds | 0:dc35364e2291 | 240 | |
krmreynolds | 0:dc35364e2291 | 241 | void minimu9::Compass_Heading() { |
krmreynolds | 0:dc35364e2291 | 242 | float MAG_X; |
krmreynolds | 0:dc35364e2291 | 243 | float MAG_Y; |
krmreynolds | 0:dc35364e2291 | 244 | float cos_roll; |
krmreynolds | 0:dc35364e2291 | 245 | float sin_roll; |
krmreynolds | 0:dc35364e2291 | 246 | float cos_pitch; |
krmreynolds | 0:dc35364e2291 | 247 | float sin_pitch; |
krmreynolds | 0:dc35364e2291 | 248 | |
krmreynolds | 0:dc35364e2291 | 249 | cos_roll = cos(roll); |
krmreynolds | 0:dc35364e2291 | 250 | sin_roll = sin(roll); |
krmreynolds | 0:dc35364e2291 | 251 | cos_pitch = cos(pitch); |
krmreynolds | 0:dc35364e2291 | 252 | sin_pitch = sin(pitch); |
krmreynolds | 0:dc35364e2291 | 253 | |
krmreynolds | 0:dc35364e2291 | 254 | // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range |
krmreynolds | 0:dc35364e2291 | 255 | c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.0; |
krmreynolds | 0:dc35364e2291 | 256 | c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*-0.5; |
krmreynolds | 0:dc35364e2291 | 257 | c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*-0.5; |
krmreynolds | 0:dc35364e2291 | 258 | |
krmreynolds | 0:dc35364e2291 | 259 | // Tilt compensated Magnetic filed X: |
krmreynolds | 0:dc35364e2291 | 260 | MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch; |
krmreynolds | 0:dc35364e2291 | 261 | // Tilt compensated Magnetic filed Y: |
krmreynolds | 0:dc35364e2291 | 262 | MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll; |
krmreynolds | 0:dc35364e2291 | 263 | // Magnetic Heading |
krmreynolds | 0:dc35364e2291 | 264 | MAG_Heading = atan2(-MAG_Y,MAG_X); |
krmreynolds | 0:dc35364e2291 | 265 | } |
krmreynolds | 0:dc35364e2291 | 266 | |
krmreynolds | 0:dc35364e2291 | 267 | void minimu9::Read_Compass() { |
krmreynolds | 0:dc35364e2291 | 268 | std::vector<short> m = compass->magRead(); |
krmreynolds | 0:dc35364e2291 | 269 | |
krmreynolds | 0:dc35364e2291 | 270 | magnetom_x = SENSOR_SIGN[6] * m[0]; |
krmreynolds | 0:dc35364e2291 | 271 | magnetom_y = SENSOR_SIGN[7] * m[1]; |
krmreynolds | 0:dc35364e2291 | 272 | magnetom_z = SENSOR_SIGN[8] * m[2]; |
krmreynolds | 0:dc35364e2291 | 273 | } |
krmreynolds | 0:dc35364e2291 | 274 | |
krmreynolds | 0:dc35364e2291 | 275 | void minimu9::Normalize(void) { |
krmreynolds | 0:dc35364e2291 | 276 | float error=0; |
krmreynolds | 0:dc35364e2291 | 277 | float temporary[3][3]; |
krmreynolds | 0:dc35364e2291 | 278 | float renorm=0; |
krmreynolds | 0:dc35364e2291 | 279 | |
krmreynolds | 0:dc35364e2291 | 280 | error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 |
krmreynolds | 0:dc35364e2291 | 281 | |
krmreynolds | 0:dc35364e2291 | 282 | Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 |
krmreynolds | 0:dc35364e2291 | 283 | Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 |
krmreynolds | 0:dc35364e2291 | 284 | |
krmreynolds | 0:dc35364e2291 | 285 | Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 |
krmreynolds | 0:dc35364e2291 | 286 | Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 |
krmreynolds | 0:dc35364e2291 | 287 | |
krmreynolds | 0:dc35364e2291 | 288 | Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 |
krmreynolds | 0:dc35364e2291 | 289 | |
krmreynolds | 0:dc35364e2291 | 290 | renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 |
krmreynolds | 0:dc35364e2291 | 291 | Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); |
krmreynolds | 0:dc35364e2291 | 292 | |
krmreynolds | 0:dc35364e2291 | 293 | renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 |
krmreynolds | 0:dc35364e2291 | 294 | Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); |
krmreynolds | 0:dc35364e2291 | 295 | |
krmreynolds | 0:dc35364e2291 | 296 | renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 |
krmreynolds | 0:dc35364e2291 | 297 | Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); |
krmreynolds | 0:dc35364e2291 | 298 | } |
krmreynolds | 0:dc35364e2291 | 299 | |
krmreynolds | 0:dc35364e2291 | 300 | /**************************************************/ |
krmreynolds | 0:dc35364e2291 | 301 | void minimu9::Drift_correction(void) { |
krmreynolds | 0:dc35364e2291 | 302 | float mag_heading_x; |
krmreynolds | 0:dc35364e2291 | 303 | float mag_heading_y; |
krmreynolds | 0:dc35364e2291 | 304 | float errorCourse; |
krmreynolds | 0:dc35364e2291 | 305 | //Compensation the Roll, Pitch and Yaw drift. |
krmreynolds | 0:dc35364e2291 | 306 | static float Scaled_Omega_P[3]; |
krmreynolds | 0:dc35364e2291 | 307 | static float Scaled_Omega_I[3]; |
krmreynolds | 0:dc35364e2291 | 308 | float Accel_magnitude; |
krmreynolds | 0:dc35364e2291 | 309 | float Accel_weight; |
krmreynolds | 0:dc35364e2291 | 310 | |
krmreynolds | 0:dc35364e2291 | 311 | |
krmreynolds | 0:dc35364e2291 | 312 | //*****Roll and Pitch*************** |
krmreynolds | 0:dc35364e2291 | 313 | |
krmreynolds | 0:dc35364e2291 | 314 | // Calculate the magnitude of the accelerometer vector |
krmreynolds | 0:dc35364e2291 | 315 | Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); |
krmreynolds | 0:dc35364e2291 | 316 | Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. |
krmreynolds | 0:dc35364e2291 | 317 | // Dynamic weighting of accelerometer info (reliability filter) |
krmreynolds | 0:dc35364e2291 | 318 | // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) |
krmreynolds | 0:dc35364e2291 | 319 | Accel_weight = 1 - 2 * abs( 1 - Accel_magnitude ); |
krmreynolds | 0:dc35364e2291 | 320 | if ( Accel_weight < 0 ) { |
krmreynolds | 0:dc35364e2291 | 321 | Accel_weight = 0; |
krmreynolds | 0:dc35364e2291 | 322 | } else if ( Accel_weight > 1 ) { |
krmreynolds | 0:dc35364e2291 | 323 | Accel_weight = 1; |
krmreynolds | 0:dc35364e2291 | 324 | } |
krmreynolds | 0:dc35364e2291 | 325 | |
krmreynolds | 0:dc35364e2291 | 326 | Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference |
krmreynolds | 0:dc35364e2291 | 327 | Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); |
krmreynolds | 0:dc35364e2291 | 328 | |
krmreynolds | 0:dc35364e2291 | 329 | Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); |
krmreynolds | 0:dc35364e2291 | 330 | Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); |
krmreynolds | 0:dc35364e2291 | 331 | |
krmreynolds | 0:dc35364e2291 | 332 | //*****YAW*************** |
krmreynolds | 0:dc35364e2291 | 333 | // We make the gyro YAW drift correction based on compass magnetic heading |
krmreynolds | 0:dc35364e2291 | 334 | |
krmreynolds | 0:dc35364e2291 | 335 | mag_heading_x = cos(MAG_Heading); |
krmreynolds | 0:dc35364e2291 | 336 | mag_heading_y = sin(MAG_Heading); |
krmreynolds | 0:dc35364e2291 | 337 | errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error |
krmreynolds | 0:dc35364e2291 | 338 | Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. |
krmreynolds | 0:dc35364e2291 | 339 | |
krmreynolds | 0:dc35364e2291 | 340 | Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01 proportional of YAW. |
krmreynolds | 0:dc35364e2291 | 341 | Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. |
krmreynolds | 0:dc35364e2291 | 342 | |
krmreynolds | 0:dc35364e2291 | 343 | Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001 Integrator |
krmreynolds | 0:dc35364e2291 | 344 | Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I |
krmreynolds | 0:dc35364e2291 | 345 | } |
krmreynolds | 0:dc35364e2291 | 346 | |
krmreynolds | 0:dc35364e2291 | 347 | void minimu9::Matrix_update(void) { |
krmreynolds | 0:dc35364e2291 | 348 | Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll |
krmreynolds | 0:dc35364e2291 | 349 | Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch |
krmreynolds | 0:dc35364e2291 | 350 | Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw |
krmreynolds | 0:dc35364e2291 | 351 | |
krmreynolds | 0:dc35364e2291 | 352 | Accel_Vector[0]=accel_x; |
krmreynolds | 0:dc35364e2291 | 353 | Accel_Vector[1]=accel_y; |
krmreynolds | 0:dc35364e2291 | 354 | Accel_Vector[2]=accel_z; |
krmreynolds | 0:dc35364e2291 | 355 | |
krmreynolds | 0:dc35364e2291 | 356 | Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term |
krmreynolds | 0:dc35364e2291 | 357 | Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term |
krmreynolds | 0:dc35364e2291 | 358 | |
krmreynolds | 0:dc35364e2291 | 359 | #if OUTPUTMODE==1 |
krmreynolds | 0:dc35364e2291 | 360 | Update_Matrix[0][0]=0; |
krmreynolds | 0:dc35364e2291 | 361 | Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z |
krmreynolds | 0:dc35364e2291 | 362 | Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y |
krmreynolds | 0:dc35364e2291 | 363 | Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z |
krmreynolds | 0:dc35364e2291 | 364 | Update_Matrix[1][1]=0; |
krmreynolds | 0:dc35364e2291 | 365 | Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x |
krmreynolds | 0:dc35364e2291 | 366 | Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y |
krmreynolds | 0:dc35364e2291 | 367 | Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x |
krmreynolds | 0:dc35364e2291 | 368 | Update_Matrix[2][2]=0; |
krmreynolds | 0:dc35364e2291 | 369 | #else // Uncorrected data (no drift correction) |
krmreynolds | 0:dc35364e2291 | 370 | Update_Matrix[0][0]=0; |
krmreynolds | 0:dc35364e2291 | 371 | Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z |
krmreynolds | 0:dc35364e2291 | 372 | Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y |
krmreynolds | 0:dc35364e2291 | 373 | Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z |
krmreynolds | 0:dc35364e2291 | 374 | Update_Matrix[1][1]=0; |
krmreynolds | 0:dc35364e2291 | 375 | Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; |
krmreynolds | 0:dc35364e2291 | 376 | Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; |
krmreynolds | 0:dc35364e2291 | 377 | Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; |
krmreynolds | 0:dc35364e2291 | 378 | Update_Matrix[2][2]=0; |
krmreynolds | 0:dc35364e2291 | 379 | #endif |
krmreynolds | 0:dc35364e2291 | 380 | |
krmreynolds | 0:dc35364e2291 | 381 | Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c |
krmreynolds | 0:dc35364e2291 | 382 | |
krmreynolds | 0:dc35364e2291 | 383 | for (int x=0; x<3; x++) { //Matrix Addition (update) |
krmreynolds | 0:dc35364e2291 | 384 | for (int y=0; y<3; y++) { |
krmreynolds | 0:dc35364e2291 | 385 | DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; |
krmreynolds | 0:dc35364e2291 | 386 | } |
krmreynolds | 0:dc35364e2291 | 387 | } |
krmreynolds | 0:dc35364e2291 | 388 | } |
krmreynolds | 0:dc35364e2291 | 389 | |
krmreynolds | 0:dc35364e2291 | 390 | void minimu9::Euler_angles(void) { |
krmreynolds | 0:dc35364e2291 | 391 | pitch = -asin(DCM_Matrix[2][0]); |
krmreynolds | 0:dc35364e2291 | 392 | roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); |
krmreynolds | 0:dc35364e2291 | 393 | yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); |
krmreynolds | 0:dc35364e2291 | 394 | } |
krmreynolds | 0:dc35364e2291 | 395 | |
krmreynolds | 0:dc35364e2291 | 396 | void minimu9::print_data(void) { |
krmreynolds | 0:dc35364e2291 | 397 | Serial serial(USBTX, USBRX); |
krmreynolds | 0:dc35364e2291 | 398 | |
krmreynolds | 0:dc35364e2291 | 399 | serial.printf("!"); |
krmreynolds | 0:dc35364e2291 | 400 | |
krmreynolds | 0:dc35364e2291 | 401 | #if PRINT_EULER == 1 |
krmreynolds | 0:dc35364e2291 | 402 | serial.printf("ANG:"); |
krmreynolds | 0:dc35364e2291 | 403 | serial.printf("%f, ", ToDeg(roll)); |
krmreynolds | 0:dc35364e2291 | 404 | serial.printf("%f, ", ToDeg(pitch)); |
krmreynolds | 0:dc35364e2291 | 405 | serial.printf("%f, ", ToDeg(yaw)); |
krmreynolds | 0:dc35364e2291 | 406 | #endif |
krmreynolds | 0:dc35364e2291 | 407 | #if PRINT_ANALOGS==1 |
krmreynolds | 0:dc35364e2291 | 408 | serial.printf(",AN:"); |
krmreynolds | 0:dc35364e2291 | 409 | serial.printf("%d, ", AN[0]); |
krmreynolds | 0:dc35364e2291 | 410 | serial.printf("%d, ", AN[1]); |
krmreynolds | 0:dc35364e2291 | 411 | serial.printf("%d, ", AN[2]); |
krmreynolds | 0:dc35364e2291 | 412 | serial.printf("%d, ", AN[3]); |
krmreynolds | 0:dc35364e2291 | 413 | serial.printf("%d, ", AN[4]); |
krmreynolds | 0:dc35364e2291 | 414 | serial.printf("%d, ", AN[5]); |
krmreynolds | 0:dc35364e2291 | 415 | serial.printf("%f, ", c_magnetom_x); |
krmreynolds | 0:dc35364e2291 | 416 | serial.printf("%f, ", c_magnetom_y); |
krmreynolds | 0:dc35364e2291 | 417 | serial.printf("%f, ", c_magnetom_z); |
krmreynolds | 0:dc35364e2291 | 418 | #endif |
krmreynolds | 0:dc35364e2291 | 419 | |
krmreynolds | 0:dc35364e2291 | 420 | #if PRINT_DCM == 1 |
krmreynolds | 0:dc35364e2291 | 421 | serial.printf (",DCM:"); |
krmreynolds | 0:dc35364e2291 | 422 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][0])); |
krmreynolds | 0:dc35364e2291 | 423 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][1])); |
krmreynolds | 0:dc35364e2291 | 424 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][2])); |
krmreynolds | 0:dc35364e2291 | 425 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][0])); |
krmreynolds | 0:dc35364e2291 | 426 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][1])); |
krmreynolds | 0:dc35364e2291 | 427 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][2])); |
krmreynolds | 0:dc35364e2291 | 428 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][0])); |
krmreynolds | 0:dc35364e2291 | 429 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][1])); |
krmreynolds | 0:dc35364e2291 | 430 | serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][2])); |
krmreynolds | 0:dc35364e2291 | 431 | #endif |
krmreynolds | 0:dc35364e2291 | 432 | serial.printf("\n"); |
krmreynolds | 0:dc35364e2291 | 433 | } |
krmreynolds | 0:dc35364e2291 | 434 | |
krmreynolds | 0:dc35364e2291 | 435 | float minimu9::get_pitch( void ) { |
krmreynolds | 0:dc35364e2291 | 436 | return ToDeg( pitch ); |
krmreynolds | 0:dc35364e2291 | 437 | } |
krmreynolds | 0:dc35364e2291 | 438 | float minimu9::get_roll( void ) { |
krmreynolds | 0:dc35364e2291 | 439 | return ToDeg( roll ); |
krmreynolds | 0:dc35364e2291 | 440 | } |
krmreynolds | 0:dc35364e2291 | 441 | float minimu9::get_yaw( void ) { |
krmreynolds | 0:dc35364e2291 | 442 | return ToDeg( yaw ); |
krmreynolds | 0:dc35364e2291 | 443 | } |
krmreynolds | 0:dc35364e2291 | 444 | |
krmreynolds | 0:dc35364e2291 | 445 | int min ( int a, int b ) { |
krmreynolds | 0:dc35364e2291 | 446 | if ( b < a ) |
krmreynolds | 0:dc35364e2291 | 447 | return b; |
krmreynolds | 0:dc35364e2291 | 448 | return a; |
krmreynolds | 0:dc35364e2291 | 449 | } |
krmreynolds | 0:dc35364e2291 | 450 | int max ( int a, int b ) { |
krmreynolds | 0:dc35364e2291 | 451 | if ( b > a ) |
krmreynolds | 0:dc35364e2291 | 452 | return b; |
krmreynolds | 0:dc35364e2291 | 453 | return a; |
krmreynolds | 0:dc35364e2291 | 454 | } |
krmreynolds | 0:dc35364e2291 | 455 | |
krmreynolds | 0:dc35364e2291 | 456 | void minimu9::Calibrate_compass( void ) { |
krmreynolds | 0:dc35364e2291 | 457 | Serial serial(USBTX, USBRX); |
krmreynolds | 0:dc35364e2291 | 458 | int running_min[3] = {2047, 2047, 2047}, running_max[3] = {-2048, -2048, -2048}; |
krmreynolds | 0:dc35364e2291 | 459 | while ( 1 ) { |
krmreynolds | 0:dc35364e2291 | 460 | std::vector<short> m = compass->magRead(); |
krmreynolds | 0:dc35364e2291 | 461 | |
krmreynolds | 0:dc35364e2291 | 462 | for ( int i = 0; i < 3; i++ ) { |
krmreynolds | 0:dc35364e2291 | 463 | running_min[i] = min(running_min[i], m[i]); |
krmreynolds | 0:dc35364e2291 | 464 | running_max[i] = max(running_max[i], m[i]); |
krmreynolds | 0:dc35364e2291 | 465 | } |
krmreynolds | 0:dc35364e2291 | 466 | |
krmreynolds | 0:dc35364e2291 | 467 | serial.printf("M min "); |
krmreynolds | 0:dc35364e2291 | 468 | serial.printf("X: %d", (int)running_min[0] ); |
krmreynolds | 0:dc35364e2291 | 469 | serial.printf(" Y: %d", (int)running_min[1]); |
krmreynolds | 0:dc35364e2291 | 470 | serial.printf(" Z: %d", (int)running_min[2]); |
krmreynolds | 0:dc35364e2291 | 471 | |
krmreynolds | 0:dc35364e2291 | 472 | serial.printf(" M max "); |
krmreynolds | 0:dc35364e2291 | 473 | serial.printf("X: %d", (int)running_max[0] ); |
krmreynolds | 0:dc35364e2291 | 474 | serial.printf(" Y: %d", (int)running_max[1]); |
krmreynolds | 0:dc35364e2291 | 475 | serial.printf(" Z: %d",(int)running_max[2]); |
krmreynolds | 0:dc35364e2291 | 476 | serial.printf("\n"); |
krmreynolds | 0:dc35364e2291 | 477 | |
krmreynolds | 0:dc35364e2291 | 478 | } |
krmreynolds | 0:dc35364e2291 | 479 | } |
krmreynolds | 0:dc35364e2291 | 480 | |
krmreynolds | 0:dc35364e2291 | 481 | void minimu9::set_calibration_values( int x_min, int y_min, int z_min, int x_max, int y_max, int z_max ) { |
krmreynolds | 0:dc35364e2291 | 482 | M_X_MIN = x_min; |
krmreynolds | 0:dc35364e2291 | 483 | M_Y_MIN = y_min; |
krmreynolds | 0:dc35364e2291 | 484 | M_Z_MIN = z_min; |
krmreynolds | 0:dc35364e2291 | 485 | M_X_MAX = x_max; |
krmreynolds | 0:dc35364e2291 | 486 | M_Y_MAX = y_max; |
krmreynolds | 0:dc35364e2291 | 487 | M_Z_MAX = z_max; |
krmreynolds | 0:dc35364e2291 | 488 | } |
krmreynolds | 0:dc35364e2291 | 489 | |
krmreynolds | 0:dc35364e2291 | 490 | void minimu9::set_print_settings( int mode, int analogs, int euler, int dcm, int serial ) { |
krmreynolds | 0:dc35364e2291 | 491 | /*For debugging purposes*/ |
krmreynolds | 0:dc35364e2291 | 492 | //OUTPUTMODE=1 will print the corrected data, |
krmreynolds | 0:dc35364e2291 | 493 | //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) |
krmreynolds | 0:dc35364e2291 | 494 | OUTPUTMODE = mode; |
krmreynolds | 0:dc35364e2291 | 495 | |
krmreynolds | 0:dc35364e2291 | 496 | //#define PRINT_DCM 0 //Will print the whole direction cosine matrix |
krmreynolds | 0:dc35364e2291 | 497 | PRINT_ANALOGS = analogs; //Will print the analog raw data |
krmreynolds | 0:dc35364e2291 | 498 | PRINT_EULER = euler; //Will print the Euler angles Roll, Pitch and Yaw |
krmreynolds | 0:dc35364e2291 | 499 | PRINT_DCM = dcm; |
krmreynolds | 0:dc35364e2291 | 500 | |
krmreynolds | 0:dc35364e2291 | 501 | PRINT_SERIAL = serial; |
krmreynolds | 0:dc35364e2291 | 502 | //PRINT_SERIAL = 1; |
krmreynolds | 0:dc35364e2291 | 503 | } |