imu for l432kc
Embed:
(wiki syntax)
Show/hide line numbers
BTL432KC.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 serial(PA_9,PA_10); 00051 Serial pc(USBTX, USBRX); // tx, rx 00052 00053 // VCC, SCE, RST, D/C, MOSI,S CLK, LED 00054 00055 //DigitalIn boardbtn(USER_BUTTON); 00056 static float acc_xyz[3]={0,0,0}; 00057 static float v_xyz[3]={0,0,0}; 00058 float displacement_body[3]={0,0,0}; 00059 static float displacement_world[3]={0,0,0}; 00060 void AccelToVelocity(float ax,float ay,float az,float dt){ 00061 static float old_acc[3]={0,0,0}; 00062 static float old_v[3]={0,0,0}; 00063 float delta_ax=ax-old_acc[0]; 00064 float delta_ay=ay-old_acc[1]; 00065 float delta_az=az-old_acc[2]; 00066 old_acc[0]=ax; 00067 old_acc[1]=ay; 00068 old_acc[2]=az; 00069 old_v[0]=v_xyz[0]-old_v[0]; 00070 old_v[1]=v_xyz[1]-old_v[1]; 00071 old_v[2]=v_xyz[2]-old_v[2]; 00072 v_xyz[0]+=ax*dt; 00073 v_xyz[1]+=ay*dt; 00074 v_xyz[2]+=az*dt; 00075 00076 00077 displacement_body[0]+=v_xyz[0]*dt; 00078 displacement_body[1]+=v_xyz[1]*dt; 00079 displacement_body[2]+=v_xyz[2]*dt; 00080 // pc.printf("%f\n",dt); 00081 /*displacement_body[0]+=fillter_vx*dt; 00082 displacement_body[1]+=fillter_vy*dt; 00083 displacement_body[2]+=fillter_vz*dt;*/ 00084 00085 } 00086 /*void VelocityToDisplacement(float dt){ 00087 displacement_body[0]+=v_xyz[0]*dt; 00088 displacement_body[1]+=v_xyz[1]*dt; 00089 displacement_body[2]+=v_xyz[2]*dt; 00090 00091 }*/ 00092 int main() 00093 { 00094 pc.baud(115200); 00095 00096 //Set up I2C 00097 i2c.frequency(400000); // use fast (400 kHz) I2C 00098 00099 // pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); 00100 00101 t.start(); 00102 00103 00104 00105 00106 // Read the WHO_AM_I register, this is a good test of communication 00107 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 00108 // pc.printf("I AM 0x%x\n\r", whoami); 00109 // pc.printf("I SHOULD BE 0x71\n\r"); 00110 00111 if (whoami == 0x71) { // WHO_AM_I should always be 0x68 00112 // pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami); 00113 // pc.printf("MPU9250 is online...\n\r"); 00114 00115 // sprintf(buffer, "0x%x", whoami); 00116 00117 wait(1); 00118 00119 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration 00120 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values 00121 /* pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); 00122 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); 00123 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); 00124 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); 00125 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); 00126 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/ 00127 // if(boardbtn==0){ 00128 // wait(2); 00129 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 00130 pc.printf("x gyro bias = %f\n\r", gyroBias[0]); 00131 pc.printf("y gyro bias = %f\n\r", gyroBias[1]); 00132 pc.printf("z gyro bias = %f\n\r", gyroBias[2]); 00133 pc.printf("x accel bias = %f\n\r", accelBias[0]); 00134 pc.printf("y accel bias = %f\n\r", accelBias[1]); 00135 pc.printf("z accel bias = %f\n\r", accelBias[2]); 00136 wait(2); 00137 00138 /* 00139 // ----------------------------------------------------------------- 00140 }else{ 00141 gyroBias[0]=-1.244275; 00142 gyroBias[1]=0.122137; 00143 gyroBias[2]=-0.717557; 00144 accelBias[0]=0.007996; // For Manual Calibate with button 00145 accelBias[1]=0.021240; 00146 accelBias[2]=-0.020508; 00147 } 00148 //------------------------------------------------------------------ 00149 */ 00150 mpu9250.initMPU9250(); 00151 // pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 00152 mpu9250.initAK8963(magCalibration); 00153 /* pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer 00154 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); 00155 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); 00156 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); 00157 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); 00158 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); 00159 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); 00160 wait(1); 00161 */ 00162 } else { 00163 pc.printf("Could not connect to MPU9250: \n\r"); 00164 pc.printf("%#x \n", whoami); 00165 00166 00167 sprintf(buffer, "WHO_AM_I 0x%x", whoami); 00168 00169 00170 while(1) ; // Loop forever if communication doesn't happen 00171 00172 } 00173 00174 mpu9250.getAres(); // Get accelerometer sensitivity 00175 mpu9250.getGres(); // Get gyro sensitivity 00176 mpu9250.getMres(); // Get magnetometer sensitivity 00177 // pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); 00178 // pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); 00179 // pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); 00180 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated 00181 magbias[1] = +120.; // User environmental x-axis correction in milliGauss 00182 magbias[2] = +125.; // User environmental x-axis correction in milliGauss 00183 00184 while(1) { 00185 00186 // If intPin goes high, all data registers have new data 00187 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt 00188 00189 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values 00190 // Now we'll calculate the accleration value into actual g's 00191 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 00192 ay = (float)accelCount[1]*aRes - accelBias[1]; 00193 az = (float)accelCount[2]*aRes - accelBias[2]; 00194 00195 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values 00196 // Calculate the gyro value into actual degrees per second 00197 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set 00198 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 00199 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 00200 00201 mpu9250.readMagData(magCount); // Read the x/y/z adc values 00202 // Calculate the magnetometer values in milliGauss 00203 // Include factory calibration per data sheet and user environmental corrections 00204 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set 00205 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; 00206 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; 00207 } 00208 00209 Now = t.read_us(); 00210 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update 00211 lastUpdate = Now; 00212 00213 sum += deltat; 00214 sumCount++; 00215 00216 // if(lastUpdate - firstUpdate > 10000000.0f) { 00217 // beta = 0.04; // decrease filter gain after stabilized 00218 // zeta = 0.015; // increasey bias drift gain after stabilized 00219 // } 00220 00221 // Pass gyro rate as rad/s 00222 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00223 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00224 00225 // Serial print and/or display at 0.5 s rate independent of data rates 00226 delt_t = t.read_ms() - count; 00227 if (delt_t > 100) { // update LCD once per half-second independent of read rate 00228 00229 /*pc.printf("ax = %f", 1000*ax); 00230 pc.printf(" ay = %f", 1000*ay); 00231 pc.printf(" az = %f mg\n\r", 1000*az); 00232 */ 00233 pc.printf("gx = %f", gx); 00234 pc.printf(" gy = %f", gy); 00235 pc.printf(" gz = %f deg/s\n\r", gz); 00236 00237 // pc.printf("gx = %f", mx); 00238 // pc.printf(" gy = %f", my); 00239 // pc.printf(" gz = %f mG\n\r", mz); 00240 00241 tempCount = mpu9250.readTempData(); // Read the adc values 00242 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade 00243 // pc.printf(" temperature = %f C\n\r", temperature); 00244 // 00245 // pc.printf("q0 = %f\n\r", q[0]); 00246 // pc.printf("q1 = %f\n\r", q[1]); 00247 // pc.printf("q2 = %f\n\r", q[2]); 00248 // pc.printf("q3 = %f\n\r", q[3]); 00249 00250 /* lcd.clear(); 00251 lcd.printString("MPU9250", 0, 0); 00252 lcd.printString("x y z", 0, 1); 00253 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az)); 00254 lcd.printString(buffer, 0, 2); 00255 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz); 00256 lcd.printString(buffer, 0, 3); 00257 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz); 00258 lcd.printString(buffer, 0, 4); 00259 */ 00260 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 00261 // In this coordinate system, the positive z-axis is down toward Earth. 00262 // 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. 00263 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 00264 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 00265 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 00266 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 00267 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 00268 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 00269 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]); 00270 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00271 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]); 00272 pitch *= 180.0f / PI; 00273 yaw *= 180.0f / PI; 00274 yaw -= 85.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 00275 roll *= 180.0f / PI; 00276 00277 // pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); 00278 // pc.printf("average rate = %f\n\r", (float) sumCount/sum); 00279 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll); 00280 // lcd.printString(buffer, 0, 4); 00281 // sprintf(buffer, "rate = %f", (float) sumCount/sum); 00282 // lcd.printString(buffer, 0, 5); 00283 00284 00285 acc_xyz[0]=ax; 00286 acc_xyz[1]=ay; 00287 acc_xyz[2]=az; 00288 float mag_a=sqrt(pow(acc_xyz[0],2)+pow(acc_xyz[1],2)+pow(acc_xyz[2],2)); 00289 AccelToVelocity(mag_a*cos(yaw),mag_a*sin(yaw),mag_a*sin(roll),float(delt_t)/10);//deltat*1000000 00290 static float fillter_vx=0; 00291 static float fillter_vy=0; 00292 static float fillter_vz=0; 00293 fillter_vx=v_xyz[0]*0.5+fillter_vx*0.5; 00294 fillter_vy=v_xyz[1]*0.5+fillter_vy*0.5; 00295 fillter_vz=v_xyz[2]*0.5+fillter_vz*0.5; 00296 //VelocityToDisplacement(float(delt_t)); 00297 //pc.printf("%f\n",sqrt(displacement_body[0]*displacement_body[0])); 00298 //pc.printf("%f %f\n",displacement_body[0],displacement_body[1]); 00299 // displacement_world[0]+=sqrt(pow(displacement_body[0],2)+pow(displacement_body[1],2)+pow(displacement_body[2],2)) 00300 // pc.printf("%f %f \n",v_xyz[0],v_xyz[1]); 00301 // pc.printf("%f %f %f\n",fillter_vx,fillter_vy,fillter_vz); 00302 myled= !myled; 00303 count = t.read_ms(); 00304 if(count > 1<<21) { 00305 t.start(); // start the timer over again if ~30 minutes has passed 00306 count = 0; 00307 deltat= 0; 00308 lastUpdate = t.read_us(); 00309 } 00310 sum = 0; 00311 sumCount = 0; 00312 //----------------------------------------------------------------------------------------- 00313 00314 if(serial.writeable()) { 00315 serial.printf("%f %f %f %f %f %f %f %f %f \n",ax,ay,az,gx,gy,gz,roll,pitch,yaw); 00316 //pc.printf("%f_%f_%f_g%f_g%f_g%f_ \n",ax,ay,az,gx,gy,gz); 00317 00318 pc.printf("%f_%f_%f \n",roll,pitch,yaw); 00319 00320 00321 } 00322 } 00323 00324 00325 } 00326 00327 } 00328
Generated on Tue Jul 12 2022 18:54:00 by
1.7.2