Oğuz Özdemir / Mbed 2 deprecated MPU9250-dmp-bluepill

Bluepill version of mpu9250-dmp library of sparkfun

Dependencies:   MotionDriver_6_1 SparkFunMPU9250-DMP mbed-STM32F103C8T6 mbed

Fork of MPU9250-dmp by Oğuz Özdemir

main.cpp@7:a862072e0981, 2017-08-14 (annotated)

Committer:
mbedoguz
Date:
Mon Aug 14 07:37:15 2017 +0000
Revision:
7:a862072e0981
Parent:
6:981cb17c7ea3
Child:
8:b9206956f396
Main program is updated to work with matlab.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedoguz 0:c6daf7016b8c 1 #include "SparkFunMPU9250-DMP.h"
mbedoguz 2:7be1ac6eb8cb 2 #include "mdcompat.h"
mbedoguz 6:981cb17c7ea3 3 #include "stm32f103c8t6.h"
mbedoguz 6:981cb17c7ea3 4 Serial pc(SERIAL_TX,SERIAL_RX);
mbedoguz 0:c6daf7016b8c 5 DigitalOut led(LED1);
mbedoguz 0:c6daf7016b8c 6 MPU9250_DMP imu;
mbedoguz 7:a862072e0981 7 //variables
mbedoguz 4:9d706f783b5a 8 unsigned char whoami[1]={0};
mbedoguz 4:9d706f783b5a 9 unsigned char fifo_count[2]={32,32};
mbedoguz 4:9d706f783b5a 10 unsigned char temp[1]={32};
mbedoguz 4:9d706f783b5a 11 unsigned char outbuff[4];
mbedoguz 5:3a9280cea2ff 12 unsigned char inbuff[4];
mbedoguz 4:9d706f783b5a 13 unsigned char regadd;
mbedoguz 5:3a9280cea2ff 14 char registeradress[5];
mbedoguz 4:9d706f783b5a 15 unsigned char registerdata[]={33};
mbedoguz 7:a862072e0981 16 unsigned long count=0;
mbedoguz 7:a862072e0981 17 //Prototypes
mbedoguz 4:9d706f783b5a 18 void printIMUData(void);
mbedoguz 0:c6daf7016b8c 19 int main()
mbedoguz 0:c6daf7016b8c 20 {
mbedoguz 0:c6daf7016b8c 21 pc.baud(115200);
mbedoguz 0:c6daf7016b8c 22 pc.printf("Hello World\n");
mbedoguz 4:9d706f783b5a 23 imu_init();
mbedoguz 4:9d706f783b5a 24 stamper_init();
mbedoguz 5:3a9280cea2ff 25 #if 1//Regular program
mbedoguz 0:c6daf7016b8c 26 // Call imu.begin() to verify communication and initialize
mbedoguz 0:c6daf7016b8c 27 if (imu.begin() != INV_SUCCESS){
mbedoguz 0:c6daf7016b8c 28 while (1){
mbedoguz 0:c6daf7016b8c 29 pc.printf("Unable to communicate with MPU-9250");
mbedoguz 0:c6daf7016b8c 30 pc.printf("Check connections, and try again.\n");
mbedoguz 0:c6daf7016b8c 31 wait_ms(5000);
mbedoguz 0:c6daf7016b8c 32 }
mbedoguz 0:c6daf7016b8c 33 }
mbedoguz 5:3a9280cea2ff 34 pc.printf("imu.begin() suceeded\n");
mbedoguz 4:9d706f783b5a 35
mbedoguz 4:9d706f783b5a 36 if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
mbedoguz 0:c6daf7016b8c 37 DMP_FEATURE_GYRO_CAL, // Use gyro calibration
mbedoguz 4:9d706f783b5a 38 20)==INV_ERROR){ // Set DMP FIFO rate to 20 Hz
mbedoguz 4:9d706f783b5a 39 pc.printf("imu.dmpBegin have failed\n");//dmpLoad function under it fails which is caused by memcmp(firmware+ii, cur, this_write) (it is located at row 2871 of inv_mpu.c)
mbedoguz 4:9d706f783b5a 40 }
mbedoguz 4:9d706f783b5a 41 else{
mbedoguz 4:9d706f783b5a 42 pc.printf("imu.dmpBegin() suceeded\n");
mbedoguz 4:9d706f783b5a 43 }
mbedoguz 7:a862072e0981 44 pc.printf("$GETEU,*\n");//indicationg that data will send to matlab by serial
mbedoguz 4:9d706f783b5a 45 #endif
mbedoguz 5:3a9280cea2ff 46 #if 0//this scope is only for debugging purposes of mbed_i2c_read and write functions
mbedoguz 4:9d706f783b5a 47 mbed_i2c_read(0x68,0x75,1,whoami);
mbedoguz 4:9d706f783b5a 48 pc.printf("whoami=%d\n",whoami[0]);
mbedoguz 5:3a9280cea2ff 49 mbed_i2c_read(0x68,0x23,2,inbuff);
mbedoguz 5:3a9280cea2ff 50 pc.printf("Buffer=%d-%d\n",inbuff[0],inbuff[1]);
mbedoguz 5:3a9280cea2ff 51 outbuff[0]=inbuff[0]&0x01;
mbedoguz 5:3a9280cea2ff 52 outbuff[1]=inbuff[1]&0x02;
mbedoguz 5:3a9280cea2ff 53 pc.printf("updated buffer=%d-%d\n",outbuff[0],outbuff[1]);
mbedoguz 4:9d706f783b5a 54 mbed_i2c_write(0x68,0x23,2,outbuff);
mbedoguz 5:3a9280cea2ff 55 mbed_i2c_read(0x68,0x23,2,inbuff);
mbedoguz 5:3a9280cea2ff 56 pc.printf("new out buffer=%d-%d(if zero, write function still needs to be fixed)\n",inbuff[0],inbuff[1]);
mbedoguz 4:9d706f783b5a 57 #endif
mbedoguz 0:c6daf7016b8c 58 while(1){
mbedoguz 4:9d706f783b5a 59 if(pc.readable()){
mbedoguz 4:9d706f783b5a 60 pc.scanf("%s",&registeradress);
mbedoguz 4:9d706f783b5a 61 regadd=(registeradress[0]-48)*100+(registeradress[1]-48)*10+(registeradress[2]-48);
mbedoguz 4:9d706f783b5a 62 mbed_i2c_read(0x68,(unsigned)regadd,1,registerdata);
mbedoguz 4:9d706f783b5a 63 pc.printf("%d is gotten from serial port, data at that register is %d\n",regadd,registerdata[0]);
mbedoguz 4:9d706f783b5a 64 }
mbedoguz 5:3a9280cea2ff 65 /*if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
mbedoguz 5:3a9280cea2ff 66 DMP_FEATURE_GYRO_CAL, // Use gyro calibration
mbedoguz 5:3a9280cea2ff 67 20)==INV_ERROR){ // Set DMP FIFO rate to 20 Hz
mbedoguz 5:3a9280cea2ff 68 pc.printf("imu.dmpBegin have failed\n");//dmpLoad function under it fails which is caused by memcmp(firmware+ii, cur, this_write) (it is located at row 2871 of inv_mpu.c)
mbedoguz 5:3a9280cea2ff 69 }
mbedoguz 5:3a9280cea2ff 70 else{
mbedoguz 5:3a9280cea2ff 71 pc.printf("imu.dmpBegin() suceeded\n");
mbedoguz 5:3a9280cea2ff 72 }*/
mbedoguz 0:c6daf7016b8c 73 // Check for new data in the FIFO
mbedoguz 3:740f15b45cb8 74 if (imu.fifoAvailable()){//fifo is not being available
mbedoguz 0:c6daf7016b8c 75 // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
mbedoguz 5:3a9280cea2ff 76 if ( imu.dmpUpdateFifo() == INV_SUCCESS){
mbedoguz 0:c6daf7016b8c 77 // computeEulerAngles can be used -- after updating the
mbedoguz 0:c6daf7016b8c 78 // quaternion values -- to estimate roll, pitch, and yaw
mbedoguz 0:c6daf7016b8c 79 imu.computeEulerAngles();
mbedoguz 5:3a9280cea2ff 80 printIMUData();
mbedoguz 0:c6daf7016b8c 81 }
mbedoguz 0:c6daf7016b8c 82 }
mbedoguz 4:9d706f783b5a 83 else{
mbedoguz 4:9d706f783b5a 84 led=1;
mbedoguz 5:3a9280cea2ff 85 wait_ms(500);
mbedoguz 4:9d706f783b5a 86 led=0;
mbedoguz 5:3a9280cea2ff 87 wait_ms(500);
mbedoguz 4:9d706f783b5a 88 }
mbedoguz 0:c6daf7016b8c 89 }
mbedoguz 0:c6daf7016b8c 90 return 0;
mbedoguz 0:c6daf7016b8c 91 }
mbedoguz 0:c6daf7016b8c 92
mbedoguz 0:c6daf7016b8c 93 void printIMUData(void)
mbedoguz 0:c6daf7016b8c 94 {
mbedoguz 0:c6daf7016b8c 95 // After calling dmpUpdateFifo() the ax, gx, mx, etc. values
mbedoguz 0:c6daf7016b8c 96 // are all updated.
mbedoguz 0:c6daf7016b8c 97 // Quaternion values are, by default, stored in Q30 long
mbedoguz 0:c6daf7016b8c 98 // format. calcQuat turns them into a float between -1 and 1
mbedoguz 0:c6daf7016b8c 99 float q0 = imu.calcQuat(imu.qw);
mbedoguz 0:c6daf7016b8c 100 float q1 = imu.calcQuat(imu.qx);
mbedoguz 0:c6daf7016b8c 101 float q2 = imu.calcQuat(imu.qy);
mbedoguz 0:c6daf7016b8c 102 float q3 = imu.calcQuat(imu.qz);
mbedoguz 0:c6daf7016b8c 103
mbedoguz 7:a862072e0981 104 pc.printf("t:%d\t Y:%lf\t P:%lf\t R:%lf\n",get_ms(&count), imu.yaw,imu.pitch,imu.roll);
mbedoguz 0:c6daf7016b8c 105 }