MPU-9250 with Kalman Filter
Dependencies: ADXL362-helloworld MPU9250_SPI mbed
Fork of ADXL362-helloworld by
Diff: main.cpp
- Revision:
- 5:abfc7660fde9
- Parent:
- 4:5a9aa5ae928a
- Child:
- 6:ea0804dc7cae
--- a/main.cpp Wed Jun 08 10:42:30 2016 +0000 +++ b/main.cpp Mon Jun 13 07:59:46 2016 +0000 @@ -33,6 +33,7 @@ adxl362[4] = new ADXL362( p12, p5, p6, p7); adxl362[5] = new ADXL362( p13, p5, p6, p7); + // we need to wait at least 500ms after ADXL362 reset printf("\r\nInitializing . \r\n"); for (int i=0; i<6; i++) { @@ -45,7 +46,7 @@ for (int i=0; i<6; i++) { - //adxl362[i]->frequency(20000); + adxl362[i]->frequency(20000); adxl362[i]->frequency(1000000); adxl362[i]->set_mode(ADXL362::MEASUREMENT); } @@ -66,6 +67,7 @@ //} uint64_t xyzt[6]; + short xi[6],yi[6],zi[6]; while(1) { @@ -94,14 +96,20 @@ for (int i=0; i<6; i++) xyzt[i] = adxl362[i]->scan(); + for (int i=0; i<6; i++) { - printf("%04x %04x %04x ", - static_cast<uint16_t>(0xFFFF&(xyzt[i]>>48)), - static_cast<uint16_t>(0xFFFF&(xyzt[i]>>32)), - static_cast<uint16_t>(0xFFFF&(xyzt[i]>>16)) - ); + uint16_t x = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>48)); + uint16_t y = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>32)); + uint16_t z = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>16)); + + if(x & 1<<16) xi[i] = -1 * static_cast<short>(~x) + 1; else xi[i] = x; + if(y & 1<<16) yi[i] = -1 * static_cast<short>(~y) + 1; else yi[i] = y; + if(z & 1<<16) zi[i] = -1 * static_cast<short>(~z) + 1; else zi[i] = z; + } + for (int i=0; i<6; i++) { + printf("%d %d %d ", xi[i], yi[i], zi[i]); } printf("\r\n"); - //wait_us(1); + wait_us(1); } }