File content as of revision 1:8715b7dd52ef:

/*

MPU9250 calibration program
This program will send data that can be used to create a file that can be used to calibrate the imus


*/

#include "mbed.h"
#include "MPU9250.h"
#include "TCA9548.h"
#include "MadgwickAHRS.h"

MPU9250 mpu9250[8];
bool validIMU[8] = {0};
Timer t;
//Serial pc(USBTX, USBRX); // tx, rx
Serial pc(USBTX, USBRX); // tx, rx

TCA9548 mux;
uint16_t timeout= 60000;    // set menu timeout to 60 seconds
const uint8_t hSize = 20;   // max number of datas to transmit
const uint8_t hLen = 10;    // max size of each header data name
bool tData[hSize] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};    // bool array to track what to transmit
float Data[hSize];
char header [hSize][hLen] = {
    {"Time(ms)"},
    {"Accel X"},
    {"Accel Y"},
    {"Accel Z"},
    {"Gyro X"},
    {"Gyro Y"},
    {"Gyro Z"},
    {"Mag X"},
    {"Mag Y"},
    {"Mag Z"},
    {"Temp"},
    {"Quatern 0"},
    {"Quatern 1"},
    {"Quatern 2"},
    {"Quatern 3"},
    {"Yaw"},
    {"Pitch"},
    {"Roll"},
    {"Checksum"},
    {"Imu"}
};

/////////////////////////////////////////////////////
//
//                  protoyptes
//
////////////////////////////////////////////////////
void menu(Serial &pc);
void getdata(Serial &pc);
void menuOptions(Serial &pc);
void calibrate(Serial &pc);
void flushSerialBuffer(Serial &pc);
void configIMU(Serial &pc);
void dispHeader(Serial &pc);
void termCLS(Serial &pc);
void transmit(Serial &pc);
void getHeader(Serial &pc);


////////////////////////////////////////////////////
//
//                      main
//
///////////////////////////////////////////////////
int main()
{
    // supported baud rates
    // 110, 300, 600, 1200, 2400, 4800, 9600,
    // 14400, 19200, 38400, 57600, 115200
    // 230400, 460800, 921600
    pc.baud(230400);
    pc.format(8, Serial::None, 1);
    //pc.set_flow_control(Serial::RTSCTS, Serial::RTS, Serial::CTS);
    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C

    t.start();

    //turn off led while everything is being configured
    myled =0;

    // Configure the IMUs
    mux.addrSelect(0);
    uint8_t imuCount = 0;
    for (uint8_t i = 0; i < 8; i++) {
        mux.addrSelect(i);
        if (mpu9250[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) == 0x71) {
            pc.printf("MPU9250 at addr %u is online...\n\r", i);
            validIMU[i] = true;
            imuCount++;
            mpu9250[i].selfTest(pc);
            mpu9250[i].config(pc);
            mpu9250[i].sensitivity(pc);
        }// end of if valid
    } // end of initalize
    if (imuCount <= 0) {
        pc.printf("No imus detected, please check wiring and reset.");
        while(1);
    }// end of no IMU's

    /*
    mpu9250[0].magbias[0] = -259.875;
    mpu9250[0].magbias[1] = 306.65;
    mpu9250[0].magbias[2] = 334.755;
    mpu9250[1].magbias[0] = -111.64;
    mpu9250[1].magbias[1] = 162.335;
    mpu9250[1].magbias[2] = -152.555;
    mpu9250[2].magbias[0] = -51.295;
    mpu9250[2].magbias[1] = 2.33;
    mpu9250[2].magbias[2] = 300.575;
    mpu9250[3].magbias[0] = 92.83;
    mpu9250[3].magbias[1] = 397.625;
    mpu9250[3].magbias[2] = 8.135;
    mpu9250[4].magbias[0] = -202.085;
    mpu9250[4].magbias[1] = 353.225;
    mpu9250[4].magbias[2] = 252.605;
    mpu9250[5].magbias[0] = -49.745;
    mpu9250[5].magbias[1] = -314.78;
    mpu9250[5].magbias[2] = 382.28;
    mpu9250[6].magbias[0] = -97.02;
    mpu9250[6].magbias[1] = 278.445;
    mpu9250[6].magbias[2] = 23.985;
    */
    /*
    mpu9250[0].magbias[0] = 528.025;
    mpu9250[0].magbias[1] = -77.24;
    mpu9250[0].magbias[2] = -364.87;
    mpu9250[1].magbias[0] = 210.35;
    mpu9250[1].magbias[1] = -60.52;
    mpu9250[1].magbias[2] = 232.01;
    mpu9250[2].magbias[0] = 226.175;
    mpu9250[2].magbias[1] = 5.445;
    mpu9250[2].magbias[2] = -451.24;
    mpu9250[3].magbias[0] = 97.445;
    mpu9250[3].magbias[1] = -185.585;
    mpu9250[3].magbias[2] = -376.69;
    mpu9250[4].magbias[0] = 428.44;
    mpu9250[4].magbias[1] = -364.695;
    mpu9250[4].magbias[2] = -550.065;
    mpu9250[5].magbias[0] = 308.285;
    mpu9250[5].magbias[1] = 668.055;
    mpu9250[5].magbias[2] = -491.725;
    mpu9250[6].magbias[0] = 83.03;
    mpu9250[6].magbias[1] = 20.205;
    mpu9250[6].magbias[2] = -151.41;
    mpu9250[7].magbias[0] = -103.81;
    mpu9250[7].magbias[1] = 35.75;
    mpu9250[7].magbias[2] = -94.875;
    */

    mpu9250[0].magbias[0] = 278.785;
    mpu9250[0].magbias[1] = 205.86;
    mpu9250[0].magbias[2] = 60.225;
    mpu9250[1].magbias[0] = 102.595;
    mpu9250[1].magbias[1] = 87.055;
    mpu9250[1].magbias[2] = 163.405;
    mpu9250[2].magbias[0] = 183.43;
    mpu9250[2].magbias[1] = -4.665;
    mpu9250[2].magbias[2] = -60.715;
    mpu9250[3].magbias[0] = 190.275;
    mpu9250[3].magbias[1] = 199.215;
    mpu9250[3].magbias[2] = -275.505;
    mpu9250[4].magbias[0] = 240.075;
    mpu9250[4].magbias[1] = -26.0;
    mpu9250[4].magbias[2] = -209.325;
    mpu9250[5].magbias[0] = 270.815;
    mpu9250[5].magbias[1] = 361.685;
    mpu9250[5].magbias[2] = -20.705;
    mpu9250[6].magbias[0] = -10.88;
    mpu9250[6].magbias[1] = 288.51;
    mpu9250[6].magbias[2] = -38.98;
    mpu9250[7].magbias[0] = -92.965;
    mpu9250[7].magbias[1] = 34.195;
    mpu9250[7].magbias[2] = -18.675;

    menu(pc);
} // end of main


////////////////////////////////////////
//
//      termCLS
//      Clears the terminal screen
//
////////////////////////////////////////
void termCLS(Serial &pc)
{
    pc.printf("\033[2J");
}


//////////////////////////////////////////
//
//      menu
//      main program menu
//
///////////////////////////////////////////
void menu(Serial &pc)
{
    pc.printf("\033[2J");
    // turn on led now that you are ready to go
    myled= 1;
    char inval =0 ;
    uint32_t stime = t.read_ms();
    pc.printf("\r\nWelcome to data logging.\r\nPlease press 'm' for a list of commands\r\n");

    while(1) {
        if ((t.read_ms() - stime) > timeout)
            menu(pc);
        inval = 0;
        if (pc.readable()) inval = pc.getc();
        //flushSerialBuffer(pc);
        switch (inval) {
            case 'm' :
                //display menu
                menuOptions(pc);
                break;

            case '0':
                menu(pc);
                break;

            case '1' :
                // start logging data
                getdata(pc);
                break;

            case '2':
                //calibrate
                calibrate(pc);
                break;

            case '3':
                configIMU(pc);
                break;

            case '4':
                getHeader(pc);
                break;

            default:
                //break;
        }
        wait(.1);
    } // end of wait to start
}// end of menu


///////////////////////////////////////////////
//
//      Menu options
//      Come back and dynamically alocate this
//
///////////////////////////////////////////////
void menuOptions(Serial &pc)
{
    termCLS(pc);
    pc.printf("'m':\tShow the menu\r\n");
    pc.printf("'0':\tReturn to this menu at anytime\r\n");
    pc.printf("'1':\tStart logging\r\n");
    pc.printf("'2':\tEnter Calibration mode\r\n");
    pc.printf("'3':\tEnter Configuration mode\r\n");
    pc.printf("'4':\tGet current data header configuration\r\n");


    pc.printf("\n");
} // end of menuOptions


/////////////////////////////////////////////
//
//      getHeader
//      Transmits header configuration over serial
//      to host data logger
//
////////////////////////////////////////////
void getHeader(Serial &pc)
{
    //termCLS(pc);
    uint8_t dcount = 0;
    for (uint8_t i = 0; i < hSize; i++)
        if (tData[i]) {
            if (dcount > 0)
                pc.printf(",");
            pc.printf("%s", header[i]);
            dcount++;
        }
    pc.printf("\r\n");
    while(1) {
        if (pc.readable())
            if (pc.getc() == '0') menu(pc);
    }
} // end of getHeader


////////////////////////////////////////////
//
//      Calibration feature, may not need
//
////////////////////////////////////////////
void calibrate(Serial &pc)
{
    // have the host computer do the following
    // save current desired data members
    // change members to only mag and imu
    // get all the datas
    // calculate calibration value
    // then write values to mbed

    //mbed then stores values as current calibration values?
    termCLS(pc);
    //pc.printf("Welcome to the calibration option\r\n");
    //pc.printf("Please make sure to run the 'Calibrate' notebook to follow calibration options\r\n");
    //pc.printf("Remember, you can press '0' at anytime to return to the main menu\r\n");

    bool newtData[hSize] = {0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,1};
    memcpy(tData, newtData, sizeof(tData));
    menu(pc);
}// end of calibrate


//////////////////////////////////////
//
//      Flush serial input buffer
//      may have issues, currently not using
//
////////////////////////////////////////////////
void flushSerialBuffer(Serial &pc)
{
    char char1 = 0;
    while (pc.readable()) {
        char1 = pc.getc();
    }
    return;
}


////////////////////////////////////////////////
//
//      Configureation menu
//      Allows for adjustable options without recompiling
//
////////////////////////////////////////////////
void configIMU(Serial &pc)
{
    char inval =0 ;
    dispHeader(pc);

    while(1) {
        inval = 0;
        if (pc.readable()) inval = pc.getc();
        //flushSerialBuffer(pc);

        switch (inval) {
            case '0' :
                //go back to main menu
                menu(pc);
                break;

            default:
                if ((inval >= 'a') && (inval <= 'a' + hSize)) {
                    tData[inval - 'a'] =! tData[inval - 'a'];
                    dispHeader(pc);
                } else if (inval != 0) {
                    pc.printf("Invalid input, please try again\r\n");
                    configIMU(pc);
                }// end of valid selection
        }// END OF SWITCH
    }// end of while
}// end of config IMU


/////////////////////////////////////////////////
//
//          Display data packet header
//
////////////////////////////////////////////////
void dispHeader(Serial &pc)
{
    termCLS(pc);
    pc.printf("Welcome to the configuration option\r\n");
    pc.printf("Below are the options you can choose for sending data\r\n");
    pc.printf("Please press the value you wish to enable / disable\r\n\n");
    pc.printf("\t0:\tReturn to main menu\r\n\n");
    for(uint8_t i = 0; i < hSize; i++) {
        pc.printf("\t%c)\t[%c]\t%s\r\n", 'a' + i, (tData[i] == 0) ? ' ': 'X', header[i]);
    }
} // end of dispHeader


///////////////////////////////////////////////////
//
//          getdata from imu and transfer
//
///////////////////////////////////////////////////
void getdata(Serial &pc)
{
    while(1) {
        if (pc.readable())
            if (pc.getc() == '0') menu(pc);
        for (uint8_t i = 0; i < 8; i++) {
            mux.addrSelect(i);
            if (!validIMU[i]) continue;
            // If intPin goes high, all data registers have new data
            if(mpu9250[i].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
                if (i==0) myled= !myled;
                // On interrupt, check if data ready interrupt
                uint32_t checksum = 0;
                mpu9250[i].readimu();


                mpu9250[i].Now = t.read_us();
                mpu9250[i].deltat = (float)((mpu9250[i].Now - mpu9250[i].lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
                mpu9250[i].lastUpdate = mpu9250[i].Now;
                MadgwickAHRSupdate(mpu9250[i].gx*PI/180.0f, mpu9250[i].gy*PI/180.0f, mpu9250[i].gz*PI/180.0f, mpu9250[i].ax, mpu9250[i].ay, mpu9250[i].az, mpu9250[i].my, mpu9250[i].mx, -1.0f*mpu9250[i].mz);
                mpu9250[i].q[0] = q0;
                mpu9250[i].q[1] = q1;
                mpu9250[i].q[2] = q2;
                mpu9250[i].q[3] = q3;

                delt_t = t.read_ms() - count;

                mpu9250[i].tempCount = mpu9250[i].readTempData();  // Read the adc values
                mpu9250[i].temperature = ((float) mpu9250[i].tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade

                mpu9250[i].roll  = atan2(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[1] + mpu9250[i].q[2] * mpu9250[i].q[3]), (1.0f - 2.0f*mpu9250[i].q[1]*mpu9250[i].q[1] - 2.0f*mpu9250[i].q[2]*mpu9250[i].q[2]));
                mpu9250[i].pitch = asin(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[2] - mpu9250[i].q[3] * mpu9250[i].q[1]));
                mpu9250[i].yaw   = atan2(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[3] + mpu9250[i].q[1] * mpu9250[i].q[2]), (1.0f - 2.0f*mpu9250[i].q[2]*mpu9250[i].q[2] - 2.0f*mpu9250[i].q[3]*mpu9250[i].q[3]));

                mpu9250[i].pitch *= 180.0f / PI;
                mpu9250[i].yaw   *= 180.0f / PI;
                mpu9250[i].roll  *= 180.0f / PI;



                transmit(pc);
                Data[1] = 1000*mpu9250[i].ax;
                Data[2] = 1000*mpu9250[i].ay;
                Data[3] = 1000*mpu9250[i].az;
                Data[4] = mpu9250[i].gx;
                Data[5] = mpu9250[i].gy;
                Data[6] = mpu9250[i].gz;
                Data[7] = mpu9250[i].mx;
                Data[8] = mpu9250[i].my;
                Data[9] = mpu9250[i].mz;

                Data[10] = mpu9250[i].temperature;
                Data[11] = mpu9250[i].q[0];
                Data[12] = mpu9250[i].q[1];
                Data[13] = mpu9250[i].q[2];
                Data[14] = mpu9250[i].q[3];
                Data[15] = mpu9250[i].yaw;
                Data[16] = mpu9250[i].pitch;
                Data[17] = mpu9250[i].roll;
                Data[18] = mpu9250[i].checksum;

                Data[19] = i;

                count = t.read_ms();

                if(count > 1<<21) {
                    t.start(); // start the timer over again if ~30 minutes has passed
                    count = 0;
                    mpu9250[i].deltat= 0;
                    mpu9250[i].lastUpdate = t.read_us();
                } // end of if
            } // end of if
        }// end of for
    } // end of while
}// end of get data


/////////////////////////////////////////
//
//          Transmit data over link
//
////////////////////////////////////////
void transmit(Serial &pc)
{
    uint8_t stuff = 0;
    if(tData[0]) {
        pc.printf("%u,", t.read_ms());
        stuff ++;
    }
    for(uint8_t i = 1; i < 18; i++)
        if (tData[i]) {
            pc.printf("%.2f,", Data[i]);
            stuff++;
        }

    if (tData[18]) {
        pc.printf("%u,", (int32_t)Data[18]);
        stuff++;
    }
    if (tData[19]) {
        pc.printf("%u", (int32_t)Data[19]);
        stuff++;
    }
    if (stuff >0) pc.printf("\r\n");
}