Veloc
Dependencies: mbed
main.cpp
00001 /* MPU9250 Basic Example Code 00002 by: Kris Winer 00003 date: April 1, 2014 00004 license: Beerware - Use this code however you'd like. If you 00005 find it useful you can buy me a beer some time. 00006 00007 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 00008 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 00009 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 00010 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. 00011 00012 SDA and SCL should have external pull-up resistors (to 3.3V). 00013 10k resistors are on the EMSENSR-9250 breakout board. 00014 00015 Hardware setup: 00016 MPU9250 Breakout --------- Arduino 00017 VDD ---------------------- 3.3V 00018 VDDI --------------------- 3.3V 00019 SDA ----------------------- A4 00020 SCL ----------------------- A5 00021 GND ---------------------- GND 00022 00023 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. 00024 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. 00025 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. 00026 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. 00027 */ 00028 00029 //#include "ST_F401_84MHZ.h" 00030 //F401_init84 myinit(0); 00031 #include "mbed.h" 00032 #include "MPU9250.h" 00033 00034 // Using NOKIA 5110 monochrome 84 x 48 pixel display 00035 // pin 9 - Serial clock out (SCLK) 00036 // pin 8 - Serial data out (DIN) 00037 // pin 7 - Data/Command select (D/C) 00038 // pin 5 - LCD chip select (CS) 00039 // pin 6 - LCD reset (RST) 00040 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); 00041 00042 float sum = 0; 00043 uint32_t sumCount = 0; 00044 char buffer[14]; 00045 00046 MPU9250 mpu9250; 00047 00048 Timer t; 00049 00050 Serial pc(USBTX, USBRX); // tx, rx 00051 00052 // VCC, SCE, RST, D/C, MOSI,S CLK, LED 00053 00054 DigitalIn boardbtn(USER_BUTTON); 00055 static float acc_xyz[3]={0,0,0}; 00056 static float v_xyz[3]={0,0,0}; 00057 float displacement_body[3]={0,0,0}; 00058 static float displacement_world[3]={0,0,0}; 00059 void AccelToVelocity(float ax,float ay,float az,float dt){ 00060 static float old_acc[3]={0,0,0}; 00061 static float old_v[3]={0,0,0}; 00062 float delta_ax=ax-old_acc[0]; 00063 float delta_ay=ay-old_acc[1]; 00064 float delta_az=az-old_acc[2]; 00065 old_acc[0]=ax; 00066 old_acc[1]=ay; 00067 old_acc[2]=az; 00068 old_v[0]=v_xyz[0]-old_v[0]; 00069 old_v[1]=v_xyz[1]-old_v[1]; 00070 old_v[2]=v_xyz[2]-old_v[2]; 00071 v_xyz[0]+=ax*dt; 00072 v_xyz[1]+=ay*dt; 00073 v_xyz[2]+=az*dt; 00074 00075 00076 displacement_body[0]+=v_xyz[0]*dt; 00077 displacement_body[1]+=v_xyz[1]*dt; 00078 displacement_body[2]+=v_xyz[2]*dt; 00079 // pc.printf("%f\n",dt); 00080 /*displacement_body[0]+=fillter_vx*dt; 00081 displacement_body[1]+=fillter_vy*dt; 00082 displacement_body[2]+=fillter_vz*dt;*/ 00083 00084 } 00085 /*void VelocityToDisplacement(float dt){ 00086 displacement_body[0]+=v_xyz[0]*dt; 00087 displacement_body[1]+=v_xyz[1]*dt; 00088 displacement_body[2]+=v_xyz[2]*dt; 00089 00090 }*/ 00091 int main() 00092 { 00093 pc.baud(9600); 00094 00095 //Set up I2C 00096 i2c.frequency(400000); // use fast (400 kHz) I2C 00097 00098 // pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); 00099 00100 t.start(); 00101 00102 00103 00104 00105 // Read the WHO_AM_I register, this is a good test of communication 00106 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 00107 // pc.printf("I AM 0x%x\n\r", whoami); 00108 // pc.printf("I SHOULD BE 0x71\n\r"); 00109 00110 if (whoami == 0x71) { // WHO_AM_I should always be 0x68 00111 // pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami); 00112 // pc.printf("MPU9250 is online...\n\r"); 00113 00114 // sprintf(buffer, "0x%x", whoami); 00115 00116 wait(1); 00117 00118 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration 00119 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values 00120 /* pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); 00121 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); 00122 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); 00123 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); 00124 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); 00125 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/ 00126 // if(boardbtn==0){ 00127 // wait(2); 00128 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 00129 pc.printf("x gyro bias = %f\n\r", gyroBias[0]); 00130 pc.printf("y gyro bias = %f\n\r", gyroBias[1]); 00131 pc.printf("z gyro bias = %f\n\r", gyroBias[2]); 00132 pc.printf("x accel bias = %f\n\r", accelBias[0]); 00133 pc.printf("y accel bias = %f\n\r", accelBias[1]); 00134 pc.printf("z accel bias = %f\n\r", accelBias[2]); 00135 wait(2); 00136 00137 /* 00138 // ----------------------------------------------------------------- 00139 }else{ 00140 gyroBias[0]=-1.244275; 00141 gyroBias[1]=0.122137; 00142 gyroBias[2]=-0.717557; 00143 accelBias[0]=0.007996; // For Manual Calibate with button 00144 accelBias[1]=0.021240; 00145 accelBias[2]=-0.020508; 00146 } 00147 //------------------------------------------------------------------ 00148 */ 00149 mpu9250.initMPU9250(); 00150 // pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 00151 mpu9250.initAK8963(magCalibration); 00152 /* pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer 00153 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); 00154 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); 00155 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); 00156 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); 00157 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); 00158 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); 00159 wait(1); 00160 */ 00161 } else { 00162 pc.printf("Could not connect to MPU9250: \n\r"); 00163 pc.printf("%#x \n", whoami); 00164 00165 00166 sprintf(buffer, "WHO_AM_I 0x%x", whoami); 00167 00168 00169 while(1) ; // Loop forever if communication doesn't happen 00170 00171 } 00172 00173 mpu9250.getAres(); // Get accelerometer sensitivity 00174 mpu9250.getGres(); // Get gyro sensitivity 00175 mpu9250.getMres(); // Get magnetometer sensitivity 00176 // pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); 00177 // pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); 00178 // pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); 00179 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated 00180 magbias[1] = +120.; // User environmental x-axis correction in milliGauss 00181 magbias[2] = +125.; // User environmental x-axis correction in milliGauss 00182 00183 while(1) { 00184 00185 // If intPin goes high, all data registers have new data 00186 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt 00187 00188 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values 00189 // Now we'll calculate the accleration value into actual g's 00190 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 00191 ay = (float)accelCount[1]*aRes - accelBias[1]; 00192 az = (float)accelCount[2]*aRes - accelBias[2]; 00193 00194 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values 00195 // Calculate the gyro value into actual degrees per second 00196 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set 00197 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 00198 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 00199 00200 mpu9250.readMagData(magCount); // Read the x/y/z adc values 00201 // Calculate the magnetometer values in milliGauss 00202 // Include factory calibration per data sheet and user environmental corrections 00203 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set 00204 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; 00205 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; 00206 } 00207 00208 Now = t.read_us(); 00209 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update 00210 lastUpdate = Now; 00211 00212 sum += deltat; 00213 sumCount++; 00214 00215 // if(lastUpdate - firstUpdate > 10000000.0f) { 00216 // beta = 0.04; // decrease filter gain after stabilized 00217 // zeta = 0.015; // increasey bias drift gain after stabilized 00218 // } 00219 00220 // Pass gyro rate as rad/s 00221 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00222 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00223 00224 // Serial print and/or display at 0.5 s rate independent of data rates 00225 delt_t = t.read_ms() - count; 00226 if (delt_t > 100) { // update LCD once per half-second independent of read rate 00227 00228 // pc.printf("ax = %f", 1000*ax); 00229 // pc.printf(" ay = %f", 1000*ay); 00230 // pc.printf(" az = %f mg\n\r", 1000*az); 00231 00232 // pc.printf("gx = %f", gx); 00233 // pc.printf(" gy = %f", gy); 00234 // pc.printf(" gz = %f deg/s\n\r", gz); 00235 00236 // pc.printf("gx = %f", mx); 00237 // pc.printf(" gy = %f", my); 00238 // pc.printf(" gz = %f mG\n\r", mz); 00239 00240 tempCount = mpu9250.readTempData(); // Read the adc values 00241 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade 00242 // pc.printf(" temperature = %f C\n\r", temperature); 00243 // 00244 // pc.printf("q0 = %f\n\r", q[0]); 00245 // pc.printf("q1 = %f\n\r", q[1]); 00246 // pc.printf("q2 = %f\n\r", q[2]); 00247 // pc.printf("q3 = %f\n\r", q[3]); 00248 00249 /* lcd.clear(); 00250 lcd.printString("MPU9250", 0, 0); 00251 lcd.printString("x y z", 0, 1); 00252 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az)); 00253 lcd.printString(buffer, 0, 2); 00254 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz); 00255 lcd.printString(buffer, 0, 3); 00256 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz); 00257 lcd.printString(buffer, 0, 4); 00258 */ 00259 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 00260 // In this coordinate system, the positive z-axis is down toward Earth. 00261 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. 00262 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 00263 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 00264 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 00265 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 00266 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 00267 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 00268 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 00269 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00270 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 00271 pitch *= 180.0f / PI; 00272 yaw *= 180.0f / PI; 00273 yaw -= 85.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 00274 roll *= 180.0f / PI; 00275 00276 // pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); 00277 // pc.printf("average rate = %f\n\r", (float) sumCount/sum); 00278 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll); 00279 // lcd.printString(buffer, 0, 4); 00280 // sprintf(buffer, "rate = %f", (float) sumCount/sum); 00281 // lcd.printString(buffer, 0, 5); 00282 00283 00284 acc_xyz[0]=ax; 00285 acc_xyz[1]=ay; 00286 acc_xyz[2]=az; 00287 float mag_a=sqrt(pow(acc_xyz[0],2)+pow(acc_xyz[1],2)+pow(acc_xyz[2],2)); 00288 AccelToVelocity(mag_a*cos(yaw),mag_a*sin(yaw),mag_a*sin(roll),float(delt_t)/10);//deltat*1000000 00289 static float fillter_vx=0; 00290 static float fillter_vy=0; 00291 static float fillter_vz=0; 00292 fillter_vx=v_xyz[0]*0.5+fillter_vx*0.5; 00293 fillter_vy=v_xyz[1]*0.5+fillter_vy*0.5; 00294 fillter_vz=v_xyz[2]*0.5+fillter_vz*0.5; 00295 //VelocityToDisplacement(float(delt_t)); 00296 //pc.printf("%f\n",sqrt(displacement_body[0]*displacement_body[0])); 00297 pc.printf("%f %f\n",displacement_body[0],displacement_body[1]); 00298 // displacement_world[0]+=sqrt(pow(displacement_body[0],2)+pow(displacement_body[1],2)+pow(displacement_body[2],2)) 00299 // pc.printf("%f %f \n",v_xyz[0],v_xyz[1]); 00300 // pc.printf("%f %f %f\n",fillter_vx,fillter_vy,fillter_vz); 00301 myled= !myled; 00302 count = t.read_ms(); 00303 if(count > 1<<21) { 00304 t.start(); // start the timer over again if ~30 minutes has passed 00305 count = 0; 00306 deltat= 0; 00307 lastUpdate = t.read_us(); 00308 } 00309 sum = 0; 00310 sumCount = 0; 00311 //----------------------------------------------------------------------------------------- 00312 00313 00314 } 00315 } 00316 00317 }
Generated on Sat Jul 16 2022 18:10:29 by
1.7.2