Masahiro Furukawa / Mbed 2 deprecated MPU-9250-Ch2

MPU-9250 with Kalman Filter

Dependencies:   ADXL362-helloworld MPU9250_SPI mbed

Fork of ADXL362-helloworld by Analog Devices

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?

UserRevisionLine numberNew 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 }