MPU-9250 with Kalman Filter
Dependencies: ADXL362-helloworld MPU9250_SPI mbed
Fork of ADXL362-helloworld by
Diff: main.cpp
- Revision:
- 4:5a9aa5ae928a
- Parent:
- 3:07aa20aa678d
- Child:
- 5:abfc7660fde9
--- a/main.cpp Mon May 23 13:46:35 2016 +0000 +++ b/main.cpp Wed Jun 08 10:42:30 2016 +0000 @@ -1,7 +1,7 @@ /** * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp * - * May 23, 2016 + * June 7, 2016 * * LDXL362 Acceleration Sensor (Extended to Ch1 ~ Ch3) * @@ -20,52 +20,88 @@ SCK (Serial Clock) p7 */ + int main() { pc.baud(115200); - DigitalOut *Ch1, *Ch2, *Ch3; - Ch1 = new DigitalOut(p8); - Ch2 = new DigitalOut(p9); - Ch3 = new DigitalOut(p10); - - *Ch1 = true; - *Ch2 = true; - *Ch3 = true; + ADXL362 *adxl362[6]; - ADXL362 adxl362(Ch2, p5, p6, p7); + adxl362[0] = new ADXL362( p8, p5, p6, p7); + adxl362[1] = new ADXL362( p9, p5, p6, p7); + adxl362[2] = new ADXL362( p10, p5, p6, p7); + adxl362[3] = new ADXL362( p11, p5, p6, p7); + 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 - adxl362.set_cs_pin(Ch1); adxl362.reset(); wait_ms(600); adxl362.set_mode(ADXL362::MEASUREMENT); - adxl362.set_cs_pin(Ch2); adxl362.reset(); wait_ms(600); adxl362.set_mode(ADXL362::MEASUREMENT); - adxl362.set_cs_pin(Ch3); adxl362.reset(); wait_ms(600); adxl362.set_mode(ADXL362::MEASUREMENT); + printf("\r\nInitializing . \r\n"); + for (int i=0; i<6; i++) { + adxl362[i]->reset(); + wait_ms(600); + } + + + wait_ms(600); - + for (int i=0; i<6; i++) + { + //adxl362[i]->frequency(20000); + adxl362[i]->frequency(1000000); + adxl362[i]->set_mode(ADXL362::MEASUREMENT); + } + //while(1) { + for (int i=0; i<6; i++) + { + uint8_t err = false; + uint8_t ad = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::DEVID_AD)); + uint8_t mst = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::DEVID_MST)); + uint8_t pid = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::PARTID)); + uint8_t rid = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::REVID)); + err |= (0xAD != ad); + err |= (0x1D != mst); + err |= (0xF2 != pid); + err |= (0x02 != rid); + if (err) printf("CH %d has error DevID_AD %02x DevID_MST %02x PartID %02x RevID %02x\r\n", i, ad, mst, pid, rid); + } + //} - uint8_t x,y,z; - uint64_t t1, t2, t3; + uint64_t xyzt[6]; while(1) { - x = adxl362.scanx_u8(); - y = adxl362.scany_u8(); - z = adxl362.scanz_u8(); - adxl362.set_cs_pin(Ch1); t1 = adxl362.scan(); - adxl362.set_cs_pin(Ch2); t2 = adxl362.scan(); - adxl362.set_cs_pin(Ch3); t3 = adxl362.scan(); + - printf("x = %02x y = %02x z = %02x %04x %04x %04x %04x %04x %04x %04x %04x %04x\r\n",x,y,z, - static_cast<uint16_t>(0xFFFF&(t1>>48)), - static_cast<uint16_t>(0xFFFF&(t1>>32)), - static_cast<uint16_t>(0xFFFF&(t1>>16)), + /* pc.putc('#'); + char ch; + for(int i=8;i>2;i--) + { + ch = static_cast<uint8_t>(0xF&( t1>>(i*8) ) ); + pc.putc(ch); + } + pc.putc(','); + for(int i=8;i>2;i--) + { + ch = static_cast<uint8_t>(0xF&( t2>>(i*8) ) ); + pc.putc(ch); + } + pc.putc(','); + for(int i=8;i>2;i--) + { + ch = static_cast<uint8_t>(0xF&( t3>>(i*8) ) ); + pc.putc(ch); + } + */ - static_cast<uint16_t>(0xFFFF&(t2>>48)), - static_cast<uint16_t>(0xFFFF&(t2>>32)), - static_cast<uint16_t>(0xFFFF&(t2>>16)), - - static_cast<uint16_t>(0xFFFF&(t3>>48)), - static_cast<uint16_t>(0xFFFF&(t3>>32)), - static_cast<uint16_t>(0xFFFF&(t3>>16)) - ); - wait_ms(1); + 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)) + ); + } + printf("\r\n"); + //wait_us(1); } }