MPU9250 library using dmp, it uses sparkfun arduino library and invensense Motion driver 6.1.
Dependencies: MotionDriver_6_1 SparkFunMPU9250-DMP mbed
main.cpp@4:9d706f783b5a, 2017-08-10 (annotated)
- Committer:
- mbedoguz
- Date:
- Thu Aug 10 14:45:40 2017 +0000
- Revision:
- 4:9d706f783b5a
- Parent:
- 3:740f15b45cb8
- Child:
- 5:3a9280cea2ff
Main function edited to find the error, and primary error is found at mdcompat.h (mbed_i2c_write) and fixed. But still dmpBegin fails at memcmp under dmpLoad. Will continue to search for it.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedoguz | 0:c6daf7016b8c | 1 | #include "SparkFunMPU9250-DMP.h" |
mbedoguz | 2:7be1ac6eb8cb | 2 | #include "mdcompat.h" |
mbedoguz | 0:c6daf7016b8c | 3 | |
mbedoguz | 0:c6daf7016b8c | 4 | Serial pc(USBTX,USBRX); |
mbedoguz | 0:c6daf7016b8c | 5 | DigitalOut led(LED1); |
mbedoguz | 0:c6daf7016b8c | 6 | MPU9250_DMP imu; |
mbedoguz | 4:9d706f783b5a | 7 | unsigned char whoami[1]={0}; |
mbedoguz | 4:9d706f783b5a | 8 | unsigned char fifo_count[2]={32,32}; |
mbedoguz | 4:9d706f783b5a | 9 | unsigned char temp[1]={32}; |
mbedoguz | 4:9d706f783b5a | 10 | unsigned char outbuff[4]; |
mbedoguz | 0:c6daf7016b8c | 11 | long DEBUG[]={2,2,2,2}; |
mbedoguz | 4:9d706f783b5a | 12 | unsigned long tamper; |
mbedoguz | 4:9d706f783b5a | 13 | unsigned char regadd; |
mbedoguz | 4:9d706f783b5a | 14 | char registeradress[20]; |
mbedoguz | 4:9d706f783b5a | 15 | unsigned char registerdata[]={33}; |
mbedoguz | 4:9d706f783b5a | 16 | void printIMUData(void); |
mbedoguz | 0:c6daf7016b8c | 17 | int main() |
mbedoguz | 0:c6daf7016b8c | 18 | { |
mbedoguz | 0:c6daf7016b8c | 19 | pc.baud(115200); |
mbedoguz | 0:c6daf7016b8c | 20 | pc.printf("Hello World\n"); |
mbedoguz | 4:9d706f783b5a | 21 | imu_init(); |
mbedoguz | 4:9d706f783b5a | 22 | stamper_init(); |
mbedoguz | 4:9d706f783b5a | 23 | #if 1 |
mbedoguz | 0:c6daf7016b8c | 24 | // Call imu.begin() to verify communication and initialize |
mbedoguz | 0:c6daf7016b8c | 25 | if (imu.begin() != INV_SUCCESS){ |
mbedoguz | 0:c6daf7016b8c | 26 | while (1){ |
mbedoguz | 0:c6daf7016b8c | 27 | pc.printf("Unable to communicate with MPU-9250"); |
mbedoguz | 0:c6daf7016b8c | 28 | pc.printf("Check connections, and try again.\n"); |
mbedoguz | 0:c6daf7016b8c | 29 | wait_ms(5000); |
mbedoguz | 0:c6daf7016b8c | 30 | } |
mbedoguz | 0:c6daf7016b8c | 31 | } |
mbedoguz | 0:c6daf7016b8c | 32 | pc.printf("imu.begin() suceeded\n"); |
mbedoguz | 4:9d706f783b5a | 33 | |
mbedoguz | 4:9d706f783b5a | 34 | if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat |
mbedoguz | 0:c6daf7016b8c | 35 | DMP_FEATURE_GYRO_CAL, // Use gyro calibration |
mbedoguz | 4:9d706f783b5a | 36 | 20)==INV_ERROR){ // Set DMP FIFO rate to 20 Hz |
mbedoguz | 4:9d706f783b5a | 37 | 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 | 38 | } |
mbedoguz | 4:9d706f783b5a | 39 | else{ |
mbedoguz | 4:9d706f783b5a | 40 | pc.printf("imu.dmpBegin() suceeded\n"); |
mbedoguz | 4:9d706f783b5a | 41 | } |
mbedoguz | 4:9d706f783b5a | 42 | #endif |
mbedoguz | 4:9d706f783b5a | 43 | #if 0//processes here is not important if imu.dmpBegin is not working -> 0 if dmpBegin fails |
mbedoguz | 4:9d706f783b5a | 44 | mbed_i2c_read(0x68,0x75,1,whoami); |
mbedoguz | 4:9d706f783b5a | 45 | pc.printf("whoami=%d\n",whoami[0]); |
mbedoguz | 4:9d706f783b5a | 46 | mbed_i2c_read(0x68,0x23,1,outbuff); |
mbedoguz | 4:9d706f783b5a | 47 | pc.printf("out buffer=%d\n",outbuff[0]); |
mbedoguz | 4:9d706f783b5a | 48 | outbuff[0]|=1; |
mbedoguz | 4:9d706f783b5a | 49 | pc.printf("updated out buffer=%d\n",outbuff[0]); |
mbedoguz | 4:9d706f783b5a | 50 | mbed_i2c_write(0x68,0x23,2,outbuff); |
mbedoguz | 4:9d706f783b5a | 51 | mbed_i2c_read(0x68,0x23,1,outbuff); |
mbedoguz | 4:9d706f783b5a | 52 | pc.printf("new out buffer=%d(if zero, write function still needs to be fixed)\n",outbuff[0]); |
mbedoguz | 4:9d706f783b5a | 53 | #endif |
mbedoguz | 0:c6daf7016b8c | 54 | while(1){ |
mbedoguz | 4:9d706f783b5a | 55 | if(pc.readable()){ |
mbedoguz | 4:9d706f783b5a | 56 | pc.scanf("%s",®isteradress); |
mbedoguz | 4:9d706f783b5a | 57 | regadd=(registeradress[0]-48)*100+(registeradress[1]-48)*10+(registeradress[2]-48); |
mbedoguz | 4:9d706f783b5a | 58 | mbed_i2c_read(0x68,(unsigned)regadd,1,registerdata); |
mbedoguz | 4:9d706f783b5a | 59 | pc.printf("%d is gotten from serial port, data at that register is %d\n",regadd,registerdata[0]); |
mbedoguz | 4:9d706f783b5a | 60 | } |
mbedoguz | 4:9d706f783b5a | 61 | /*unsigned char buff[2]={0x38,0x39}; |
mbedoguz | 4:9d706f783b5a | 62 | pc.printf("trial-%s-%s\n",buff,(const char*)buff);*/ |
mbedoguz | 4:9d706f783b5a | 63 | /*imu.updateTemperature(); |
mbedoguz | 4:9d706f783b5a | 64 | temp[0]=imu.temperature; |
mbedoguz | 4:9d706f783b5a | 65 | get_ms(&tamper); |
mbedoguz | 4:9d706f783b5a | 66 | pc.printf("%d-%d\n",temp[0],tamper);*/ |
mbedoguz | 0:c6daf7016b8c | 67 | // Check for new data in the FIFO |
mbedoguz | 3:740f15b45cb8 | 68 | if (imu.fifoAvailable()){//fifo is not being available |
mbedoguz | 0:c6daf7016b8c | 69 | led=0; |
mbedoguz | 4:9d706f783b5a | 70 | wait_ms(500); |
mbedoguz | 0:c6daf7016b8c | 71 | led=1; |
mbedoguz | 4:9d706f783b5a | 72 | wait_ms(500); |
mbedoguz | 0:c6daf7016b8c | 73 | // Use dmpUpdateFifo to update the ax, gx, mx, etc. values |
mbedoguz | 0:c6daf7016b8c | 74 | if ( imu.dmpUpdateFifo(DEBUG) == INV_SUCCESS){ |
mbedoguz | 0:c6daf7016b8c | 75 | // computeEulerAngles can be used -- after updating the |
mbedoguz | 0:c6daf7016b8c | 76 | // quaternion values -- to estimate roll, pitch, and yaw |
mbedoguz | 0:c6daf7016b8c | 77 | imu.computeEulerAngles(); |
mbedoguz | 0:c6daf7016b8c | 78 | //printIMUData(); |
mbedoguz | 3:740f15b45cb8 | 79 | pc.printf("dmpUpdateFifo sucesfully occured\n"); |
mbedoguz | 0:c6daf7016b8c | 80 | #if 1 |
mbedoguz | 0:c6daf7016b8c | 81 | pc.printf("%lf",DEBUG[0]); |
mbedoguz | 0:c6daf7016b8c | 82 | pc.printf("\t"); |
mbedoguz | 0:c6daf7016b8c | 83 | pc.printf("%lf",DEBUG[1]); |
mbedoguz | 0:c6daf7016b8c | 84 | pc.printf("\t"); |
mbedoguz | 0:c6daf7016b8c | 85 | pc.printf("%lf",DEBUG[2]); |
mbedoguz | 0:c6daf7016b8c | 86 | pc.printf("\t"); |
mbedoguz | 0:c6daf7016b8c | 87 | pc.printf("%lf\n ",DEBUG[3]); |
mbedoguz | 0:c6daf7016b8c | 88 | #endif |
mbedoguz | 0:c6daf7016b8c | 89 | } |
mbedoguz | 0:c6daf7016b8c | 90 | } |
mbedoguz | 4:9d706f783b5a | 91 | else{ |
mbedoguz | 4:9d706f783b5a | 92 | led=1; |
mbedoguz | 4:9d706f783b5a | 93 | wait_ms(100); |
mbedoguz | 4:9d706f783b5a | 94 | led=0; |
mbedoguz | 4:9d706f783b5a | 95 | wait_ms(100); |
mbedoguz | 4:9d706f783b5a | 96 | } |
mbedoguz | 0:c6daf7016b8c | 97 | } |
mbedoguz | 0:c6daf7016b8c | 98 | return 0; |
mbedoguz | 0:c6daf7016b8c | 99 | } |
mbedoguz | 0:c6daf7016b8c | 100 | |
mbedoguz | 0:c6daf7016b8c | 101 | void printIMUData(void) |
mbedoguz | 0:c6daf7016b8c | 102 | { |
mbedoguz | 0:c6daf7016b8c | 103 | // After calling dmpUpdateFifo() the ax, gx, mx, etc. values |
mbedoguz | 0:c6daf7016b8c | 104 | // are all updated. |
mbedoguz | 0:c6daf7016b8c | 105 | // Quaternion values are, by default, stored in Q30 long |
mbedoguz | 0:c6daf7016b8c | 106 | // format. calcQuat turns them into a float between -1 and 1 |
mbedoguz | 0:c6daf7016b8c | 107 | float q0 = imu.calcQuat(imu.qw); |
mbedoguz | 0:c6daf7016b8c | 108 | float q1 = imu.calcQuat(imu.qx); |
mbedoguz | 0:c6daf7016b8c | 109 | float q2 = imu.calcQuat(imu.qy); |
mbedoguz | 0:c6daf7016b8c | 110 | float q3 = imu.calcQuat(imu.qz); |
mbedoguz | 0:c6daf7016b8c | 111 | |
mbedoguz | 0:c6daf7016b8c | 112 | pc.printf("R:%lf P:%lf Y:%lf \n", imu.roll,imu.pitch,imu.yaw); |
mbedoguz | 0:c6daf7016b8c | 113 | } |