MPU-9250 with Kalman Filter

Dependencies:   ADXL362-helloworld MPU9250_SPI mbed

Fork of ADXL362-helloworld by Analog Devices

main.cpp

Committer:
mfurukawa
Date:
2016-06-08
Revision:
4:5a9aa5ae928a
Parent:
3:07aa20aa678d
Child:
5:abfc7660fde9

File content as of revision 4:5a9aa5ae928a:

/**
 * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
 * 
 * June 7, 2016
 *
 * LDXL362 Acceleration Sensor (Extended to Ch1 ~ Ch3)
 *
 **/


#include "mbed.h"
#include "ADXL362.h"

Serial pc(USBTX, USBRX);
 
/*
    ~CS  (Chip Select)          p8
    MOSI (Master Out Slave In)  p5
    MISO (Master In Slave Out   p6
    SCK  (Serial Clock)         p7
*/


int main() {
    pc.baud(115200);
    
    ADXL362 *adxl362[6];
    
    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
    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); 
    }
    //}
    
    uint64_t xyzt[6];
    
    while(1) {
        
           
        
 /*     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);
        }
    */     
        
        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);
    }
}