MPU-9250 with Kalman Filter
Dependencies: ADXL362-helloworld MPU9250_SPI mbed
Fork of ADXL362-helloworld by
main.cpp@6:ea0804dc7cae, 2016-06-17 (annotated)
- Committer:
- mfurukawa
- Date:
- Fri Jun 17 05:16:13 2016 +0000
- Revision:
- 6:ea0804dc7cae
- Parent:
- 5:abfc7660fde9
- Child:
- 7:758a94e02aa7
initial
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mfurukawa | 3:07aa20aa678d | 1 | /** |
mfurukawa | 3:07aa20aa678d | 2 | * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp |
mfurukawa | 3:07aa20aa678d | 3 | * |
mfurukawa | 6:ea0804dc7cae | 4 | * June 17, 2016 |
mfurukawa | 3:07aa20aa678d | 5 | * |
mfurukawa | 6:ea0804dc7cae | 6 | * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2) |
mfurukawa | 3:07aa20aa678d | 7 | * |
mfurukawa | 3:07aa20aa678d | 8 | **/ |
mfurukawa | 3:07aa20aa678d | 9 | |
mfurukawa | 1:f1e4ee4fc335 | 10 | |
adisuciu | 0:83fda1bfaffe | 11 | #include "mbed.h" |
mfurukawa | 6:ea0804dc7cae | 12 | #include "MPU9250.h" |
mfurukawa | 1:f1e4ee4fc335 | 13 | |
mfurukawa | 1:f1e4ee4fc335 | 14 | /* |
mfurukawa | 1:f1e4ee4fc335 | 15 | MOSI (Master Out Slave In) p5 |
mfurukawa | 1:f1e4ee4fc335 | 16 | MISO (Master In Slave Out p6 |
mfurukawa | 1:f1e4ee4fc335 | 17 | SCK (Serial Clock) p7 |
mfurukawa | 6:ea0804dc7cae | 18 | ~CS (Chip Select) p8 |
mfurukawa | 1:f1e4ee4fc335 | 19 | */ |
adisuciu | 0:83fda1bfaffe | 20 | |
mfurukawa | 6:ea0804dc7cae | 21 | // https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp |
mfurukawa | 4:5a9aa5ae928a | 22 | |
adisuciu | 0:83fda1bfaffe | 23 | int main() { |
mfurukawa | 6:ea0804dc7cae | 24 | |
mfurukawa | 6:ea0804dc7cae | 25 | Serial pc(USBTX, USBRX); |
mfurukawa | 2:9ef7a594159c | 26 | pc.baud(115200); |
adisuciu | 0:83fda1bfaffe | 27 | |
mfurukawa | 6:ea0804dc7cae | 28 | SPI spi(p5, p6, p7); |
mfurukawa | 6:ea0804dc7cae | 29 | |
mfurukawa | 6:ea0804dc7cae | 30 | //define the mpu9250 object |
mfurukawa | 6:ea0804dc7cae | 31 | mpu9250_spi *imu[2]; |
mfurukawa | 3:07aa20aa678d | 32 | |
mfurukawa | 6:ea0804dc7cae | 33 | imu[0] = new mpu9250_spi(spi, p8); |
mfurukawa | 6:ea0804dc7cae | 34 | imu[1] = new mpu9250_spi(spi, p9); |
mfurukawa | 6:ea0804dc7cae | 35 | |
mfurukawa | 6:ea0804dc7cae | 36 | int i=0; |
mfurukawa | 6:ea0804dc7cae | 37 | |
mfurukawa | 6:ea0804dc7cae | 38 | if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250 |
mfurukawa | 6:ea0804dc7cae | 39 | printf("\nCouldn't initialize MPU9250 via SPI!"); |
mfurukawa | 6:ea0804dc7cae | 40 | } |
mfurukawa | 6:ea0804dc7cae | 41 | printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104 |
mfurukawa | 6:ea0804dc7cae | 42 | wait(1); |
mfurukawa | 6:ea0804dc7cae | 43 | printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros |
mfurukawa | 6:ea0804dc7cae | 44 | wait(1); |
mfurukawa | 6:ea0804dc7cae | 45 | printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G)); //Set full scale range for accs |
mfurukawa | 6:ea0804dc7cae | 46 | wait(1); |
mfurukawa | 6:ea0804dc7cae | 47 | printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami()); |
mfurukawa | 6:ea0804dc7cae | 48 | wait(0.1); |
mfurukawa | 6:ea0804dc7cae | 49 | imu[i]->AK8963_calib_Magnetometer(); |
mfurukawa | 6:ea0804dc7cae | 50 | |
mfurukawa | 6:ea0804dc7cae | 51 | while(1) { |
mfurukawa | 6:ea0804dc7cae | 52 | //myled = 1; |
mfurukawa | 6:ea0804dc7cae | 53 | wait(0.1); |
mfurukawa | 6:ea0804dc7cae | 54 | /* |
mfurukawa | 6:ea0804dc7cae | 55 | imu[i]->read_temp(); |
mfurukawa | 6:ea0804dc7cae | 56 | imu[i]->read_acc(); |
mfurukawa | 6:ea0804dc7cae | 57 | imu[i]->read_rot(); |
mfurukawa | 6:ea0804dc7cae | 58 | imu[i]->AK8963_read_Magnetometer(); |
mfurukawa | 6:ea0804dc7cae | 59 | */ |
mfurukawa | 6:ea0804dc7cae | 60 | imu[i]->read_all(); |
mfurukawa | 6:ea0804dc7cae | 61 | printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f\n", |
mfurukawa | 6:ea0804dc7cae | 62 | imu[i]->Temperature, |
mfurukawa | 6:ea0804dc7cae | 63 | imu[i]->gyroscope_data[0], |
mfurukawa | 6:ea0804dc7cae | 64 | imu[i]->gyroscope_data[1], |
mfurukawa | 6:ea0804dc7cae | 65 | imu[i]->gyroscope_data[2], |
mfurukawa | 6:ea0804dc7cae | 66 | imu[i]->accelerometer_data[0], |
mfurukawa | 6:ea0804dc7cae | 67 | imu[i]->accelerometer_data[1], |
mfurukawa | 6:ea0804dc7cae | 68 | imu[i]->accelerometer_data[2], |
mfurukawa | 6:ea0804dc7cae | 69 | imu[i]->Magnetometer[0], |
mfurukawa | 6:ea0804dc7cae | 70 | imu[i]->Magnetometer[1], |
mfurukawa | 6:ea0804dc7cae | 71 | imu[i]->Magnetometer[2] |
mfurukawa | 6:ea0804dc7cae | 72 | ); |
mfurukawa | 6:ea0804dc7cae | 73 | //myled = 0; |
mfurukawa | 6:ea0804dc7cae | 74 | //wait(0.5); |
mfurukawa | 6:ea0804dc7cae | 75 | } |
mfurukawa | 6:ea0804dc7cae | 76 | |
mfurukawa | 6:ea0804dc7cae | 77 | /* |
mfurukawa | 4:5a9aa5ae928a | 78 | uint64_t xyzt[6]; |
mfurukawa | 5:abfc7660fde9 | 79 | short xi[6],yi[6],zi[6]; |
mfurukawa | 2:9ef7a594159c | 80 | |
adisuciu | 0:83fda1bfaffe | 81 | while(1) { |
mfurukawa | 3:07aa20aa678d | 82 | |
mfurukawa | 3:07aa20aa678d | 83 | |
mfurukawa | 4:5a9aa5ae928a | 84 | for (int i=0; i<6; i++) |
mfurukawa | 4:5a9aa5ae928a | 85 | xyzt[i] = adxl362[i]->scan(); |
mfurukawa | 5:abfc7660fde9 | 86 | |
mfurukawa | 4:5a9aa5ae928a | 87 | for (int i=0; i<6; i++) { |
mfurukawa | 5:abfc7660fde9 | 88 | uint16_t x = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>48)); |
mfurukawa | 5:abfc7660fde9 | 89 | uint16_t y = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>32)); |
mfurukawa | 5:abfc7660fde9 | 90 | uint16_t z = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>16)); |
mfurukawa | 5:abfc7660fde9 | 91 | |
mfurukawa | 5:abfc7660fde9 | 92 | if(x & 1<<16) xi[i] = -1 * static_cast<short>(~x) + 1; else xi[i] = x; |
mfurukawa | 5:abfc7660fde9 | 93 | if(y & 1<<16) yi[i] = -1 * static_cast<short>(~y) + 1; else yi[i] = y; |
mfurukawa | 5:abfc7660fde9 | 94 | if(z & 1<<16) zi[i] = -1 * static_cast<short>(~z) + 1; else zi[i] = z; |
mfurukawa | 5:abfc7660fde9 | 95 | } |
mfurukawa | 5:abfc7660fde9 | 96 | for (int i=0; i<6; i++) { |
mfurukawa | 5:abfc7660fde9 | 97 | printf("%d %d %d ", xi[i], yi[i], zi[i]); |
mfurukawa | 4:5a9aa5ae928a | 98 | } |
mfurukawa | 4:5a9aa5ae928a | 99 | printf("\r\n"); |
mfurukawa | 5:abfc7660fde9 | 100 | wait_us(1); |
adisuciu | 0:83fda1bfaffe | 101 | } |
mfurukawa | 6:ea0804dc7cae | 102 | */ |
adisuciu | 0:83fda1bfaffe | 103 | } |