MPU9250 library using dmp, it uses sparkfun arduino library and invensense Motion driver 6.1.
Dependencies: MotionDriver_6_1 SparkFunMPU9250-DMP mbed
main.cpp@2:7be1ac6eb8cb, 2017-08-08 (annotated)
- Committer:
- mbedoguz
- Date:
- Tue Aug 08 08:39:57 2017 +0000
- Revision:
- 2:7be1ac6eb8cb
- Parent:
- 0:c6daf7016b8c
- Child:
- 3:740f15b45cb8
Debug friendly code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedoguz | 0:c6daf7016b8c | 1 | /*#include "mbed.h" |
mbedoguz | 0:c6daf7016b8c | 2 | #include "inv_mpu.h" |
mbedoguz | 0:c6daf7016b8c | 3 | #include "mdcompat.h" |
mbedoguz | 0:c6daf7016b8c | 4 | Serial pc(USBTX,USBRX); |
mbedoguz | 0:c6daf7016b8c | 5 | DigitalOut led(LED1); |
mbedoguz | 0:c6daf7016b8c | 6 | int main() |
mbedoguz | 0:c6daf7016b8c | 7 | { |
mbedoguz | 0:c6daf7016b8c | 8 | imu_init(); |
mbedoguz | 0:c6daf7016b8c | 9 | |
mbedoguz | 0:c6daf7016b8c | 10 | while(1){ |
mbedoguz | 0:c6daf7016b8c | 11 | led=1; |
mbedoguz | 0:c6daf7016b8c | 12 | wait_ms(100); |
mbedoguz | 0:c6daf7016b8c | 13 | led=0; |
mbedoguz | 0:c6daf7016b8c | 14 | wait_ms(100); |
mbedoguz | 0:c6daf7016b8c | 15 | } |
mbedoguz | 0:c6daf7016b8c | 16 | }*/ |
mbedoguz | 0:c6daf7016b8c | 17 | |
mbedoguz | 0:c6daf7016b8c | 18 | #include "SparkFunMPU9250-DMP.h" |
mbedoguz | 2:7be1ac6eb8cb | 19 | #include "mdcompat.h" |
mbedoguz | 0:c6daf7016b8c | 20 | |
mbedoguz | 0:c6daf7016b8c | 21 | Serial pc(USBTX,USBRX); |
mbedoguz | 0:c6daf7016b8c | 22 | DigitalOut led(LED1); |
mbedoguz | 0:c6daf7016b8c | 23 | MPU9250_DMP imu; |
mbedoguz | 2:7be1ac6eb8cb | 24 | char datax[1]={0}; |
mbedoguz | 0:c6daf7016b8c | 25 | long DEBUG[]={2,2,2,2}; |
mbedoguz | 0:c6daf7016b8c | 26 | int main() |
mbedoguz | 0:c6daf7016b8c | 27 | { |
mbedoguz | 0:c6daf7016b8c | 28 | pc.baud(115200); |
mbedoguz | 0:c6daf7016b8c | 29 | pc.printf("Hello World\n"); |
mbedoguz | 0:c6daf7016b8c | 30 | // Call imu.begin() to verify communication and initialize |
mbedoguz | 0:c6daf7016b8c | 31 | pc.printf("imu.begin()is being run\n"); |
mbedoguz | 0:c6daf7016b8c | 32 | if (imu.begin() != INV_SUCCESS){ |
mbedoguz | 0:c6daf7016b8c | 33 | pc.printf("imu.begin() have failed"); |
mbedoguz | 0:c6daf7016b8c | 34 | while (1){ |
mbedoguz | 0:c6daf7016b8c | 35 | pc.printf("Unable to communicate with MPU-9250"); |
mbedoguz | 0:c6daf7016b8c | 36 | pc.printf("Check connections, and try again.\n"); |
mbedoguz | 0:c6daf7016b8c | 37 | wait_ms(5000); |
mbedoguz | 0:c6daf7016b8c | 38 | } |
mbedoguz | 0:c6daf7016b8c | 39 | } |
mbedoguz | 0:c6daf7016b8c | 40 | pc.printf("imu.begin() suceeded\n"); |
mbedoguz | 0:c6daf7016b8c | 41 | imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat |
mbedoguz | 0:c6daf7016b8c | 42 | DMP_FEATURE_GYRO_CAL, // Use gyro calibration |
mbedoguz | 0:c6daf7016b8c | 43 | 20); // Set DMP FIFO rate to 20 Hz |
mbedoguz | 0:c6daf7016b8c | 44 | // DMP_FEATURE_LP_QUAT can also be used. It uses the |
mbedoguz | 0:c6daf7016b8c | 45 | // accelerometer in low-power mode to estimate quat's. |
mbedoguz | 0:c6daf7016b8c | 46 | // DMP_FEATURE_LP_QUAT and 6X_LP_QUAT are mutually exclusive |
mbedoguz | 0:c6daf7016b8c | 47 | pc.printf("imu.dmpBegin() suceeded\n"); |
mbedoguz | 0:c6daf7016b8c | 48 | |
mbedoguz | 0:c6daf7016b8c | 49 | while(1){ |
mbedoguz | 0:c6daf7016b8c | 50 | // Check for new data in the FIFO |
mbedoguz | 0:c6daf7016b8c | 51 | led=1; |
mbedoguz | 0:c6daf7016b8c | 52 | wait_ms(500); |
mbedoguz | 0:c6daf7016b8c | 53 | led=0; |
mbedoguz | 0:c6daf7016b8c | 54 | wait_ms(500); |
mbedoguz | 2:7be1ac6eb8cb | 55 | mbed_i2c_read((unsigned char)0x68,(unsigned char)0x75,1,datax); |
mbedoguz | 2:7be1ac6eb8cb | 56 | pc.printf("%d",datax[0]); |
mbedoguz | 0:c6daf7016b8c | 57 | if ( imu.fifoAvailable() ){//fifo is not being available |
mbedoguz | 0:c6daf7016b8c | 58 | led=0; |
mbedoguz | 0:c6daf7016b8c | 59 | wait_ms(100); |
mbedoguz | 0:c6daf7016b8c | 60 | led=1; |
mbedoguz | 0:c6daf7016b8c | 61 | wait_ms(100); |
mbedoguz | 0:c6daf7016b8c | 62 | // Use dmpUpdateFifo to update the ax, gx, mx, etc. values |
mbedoguz | 0:c6daf7016b8c | 63 | if ( imu.dmpUpdateFifo(DEBUG) == INV_SUCCESS){ |
mbedoguz | 0:c6daf7016b8c | 64 | // computeEulerAngles can be used -- after updating the |
mbedoguz | 0:c6daf7016b8c | 65 | // quaternion values -- to estimate roll, pitch, and yaw |
mbedoguz | 0:c6daf7016b8c | 66 | imu.computeEulerAngles(); |
mbedoguz | 0:c6daf7016b8c | 67 | //printIMUData(); |
mbedoguz | 0:c6daf7016b8c | 68 | #if 1 |
mbedoguz | 0:c6daf7016b8c | 69 | pc.printf("%lf",DEBUG[0]); |
mbedoguz | 0:c6daf7016b8c | 70 | pc.printf("\t"); |
mbedoguz | 0:c6daf7016b8c | 71 | pc.printf("%lf",DEBUG[1]); |
mbedoguz | 0:c6daf7016b8c | 72 | pc.printf("\t"); |
mbedoguz | 0:c6daf7016b8c | 73 | pc.printf("%lf",DEBUG[2]); |
mbedoguz | 0:c6daf7016b8c | 74 | pc.printf("\t"); |
mbedoguz | 0:c6daf7016b8c | 75 | pc.printf("%lf\n ",DEBUG[3]); |
mbedoguz | 0:c6daf7016b8c | 76 | #endif |
mbedoguz | 0:c6daf7016b8c | 77 | } |
mbedoguz | 0:c6daf7016b8c | 78 | } |
mbedoguz | 0:c6daf7016b8c | 79 | } |
mbedoguz | 0:c6daf7016b8c | 80 | return 0; |
mbedoguz | 0:c6daf7016b8c | 81 | } |
mbedoguz | 0:c6daf7016b8c | 82 | |
mbedoguz | 0:c6daf7016b8c | 83 | void printIMUData(void) |
mbedoguz | 0:c6daf7016b8c | 84 | { |
mbedoguz | 0:c6daf7016b8c | 85 | // After calling dmpUpdateFifo() the ax, gx, mx, etc. values |
mbedoguz | 0:c6daf7016b8c | 86 | // are all updated. |
mbedoguz | 0:c6daf7016b8c | 87 | // Quaternion values are, by default, stored in Q30 long |
mbedoguz | 0:c6daf7016b8c | 88 | // format. calcQuat turns them into a float between -1 and 1 |
mbedoguz | 0:c6daf7016b8c | 89 | float q0 = imu.calcQuat(imu.qw); |
mbedoguz | 0:c6daf7016b8c | 90 | float q1 = imu.calcQuat(imu.qx); |
mbedoguz | 0:c6daf7016b8c | 91 | float q2 = imu.calcQuat(imu.qy); |
mbedoguz | 0:c6daf7016b8c | 92 | float q3 = imu.calcQuat(imu.qz); |
mbedoguz | 0:c6daf7016b8c | 93 | |
mbedoguz | 0:c6daf7016b8c | 94 | pc.printf("R:%lf P:%lf Y:%lf \n", imu.roll,imu.pitch,imu.yaw); |
mbedoguz | 0:c6daf7016b8c | 95 | } |