File content as of revision 0:c5fbc6927336:

#include "mbed.h"
#include "UM6LT.h"

Serial pc(USBTX, USBRX);
UM6LT imu(p28, p27);

int stdDelayMs = 2;

int main() {
    
    pc.baud(115200);
    imu.baud(115200);

    int broadcastRate = 22;
    int baudrate = 115200;
    
    int wantCov = 0;
    int wantEulerAngles = 0;
    int wantQuat = 0;
    int wantProcMag = 0;
    int wantProcAccel = 0;
    int wantProcGyro = 0;
    int wantRawMag = 0;
    int wantRawAccel = 0;
    int wantRawGyro = 0;
        
    int dataToTransmit [9] = {wantCov, wantEulerAngles, wantQuat, wantProcMag, wantProcAccel, wantProcGyro, wantRawMag, wantRawAccel, wantRawGyro};
    int broadcastEnabled = 0;
    
    imu.setCommParams(broadcastRate, baudrate, dataToTransmit, broadcastEnabled);
    
    int wantPPS = 0;
    int wantQuatUpdate = 1;
    int wantCal = 1;
    int wantAccelUpdate = 1;
    int wantMagUpdate = 1;
    
    imu.setConfigParams(wantPPS, wantQuatUpdate, wantCal, wantAccelUpdate, wantMagUpdate);
    
    if(imu.getStatus()){
        
        int roll = 0;
        int pitch = 0;
        int yaw = 0;
        int rollRate = 0;
        int pitchRate = 0;
        int yawRate = 0;
        int accelX = 0 ;
        int accelY = 0 ;
        int accelZ = 0 ;
        int magX = 0;
        int magY = 0;
        int magZ = 0;
        int gyroBiasX = 0;
        int gyroBiasY = 0;
        int gyroBiasZ = 0;
        int a = 0;
        int b = 0;
        int c = 0;
        int d = 0;
        
        int byteBuffer;
        
        for(int i=0; i<5; i++){
            if(imu.getAngles(roll, pitch, yaw)){
                wait_ms(stdDelayMs);
                printf("roll: %d   pitch: %d   yaw: %d\r\n", roll, pitch, yaw);
            }
        }
        
        printf("\r\n\r\nAngles should be random but consistent\r\n\r\n");
        
        if(imu.zeroGyros(gyroBiasX, gyroBiasY,gyroBiasZ)){
            printf("Gyro Bias X: %d  Gyro Bias Y: %d  Gyro Bias Z: %d\r\n", gyroBiasX, gyroBiasY, gyroBiasZ); 
        }
        
        if(imu.autoSetAccelRef() && imu.autoSetMagRef() && imu.resetEKF()){
        
            printf("Press (1) for Euler Angles.\r\n");
            printf("Press (2) for Accelerations.\r\n");
            printf("Press (3) for Magnetic field.\r\n");
            printf("Press (4) for Angle rates.\r\n");
            printf("Press (5) for Quaternion.\r\n");
            printf("Press (0) to stop.\r\n");
            
            while(!pc.readable()){
                wait_ms(stdDelayMs);
            }            
            byteBuffer = pc.getc();
        
            while(1){
            
                if(pc.readable()){                    
                    byteBuffer = pc.getc();
                }
                
                switch(byteBuffer){
                    case '0':
                        wait_ms(stdDelayMs);
                        break;
                    case '1':
                        if(imu.getAngles(roll, pitch, yaw)){
                            wait_ms(stdDelayMs);
                            printf("roll: %d   pitch: %d   yaw: %d\r\n", roll, pitch, yaw);
                        }
                        break;
                    case '2':
                        if(imu.getAccel(accelX, accelY, accelZ)){
                            wait_ms(stdDelayMs);
                            printf("accelX: %d   accelY: %d   accelZ: %d\r\n", accelX, accelY, accelZ);
                        }
                        break;
                    case '3':
                        if(imu.getMag(magX, magY, magZ)){
                            wait_ms(stdDelayMs);
                            printf("magX: %d   magY: %d   magZ: %d\r\n", magX, magY, magZ);
                        }
                        break;
                    case '4':
                        if(imu.getAngleRates(rollRate, pitchRate, yawRate)){
                            wait_ms(stdDelayMs);
                            printf("rollRate: %d   pitchRate: %d   yawRate: %d\r\n", rollRate, pitchRate, yawRate);
                        }
                        break;
                    case '5':
                        if(imu.getQuaternion(a, b, c, d)){
                            wait_ms(stdDelayMs);
                            printf("a: %d  b: %d  c: %d  d: %d\r\n", a, b, c, d);
                        }
                        break;
                    default:
                        printf("Wrong command. Enter '1', '2', '3', '4', '5' or '6'.\r\n");
                        byteBuffer = '0';
                        break;
                }
            }
            
        }    
    }    
}