File content as of revision 0:8c28fac22d27:

#include "setup.h"
#include "tasks.h"

Serial BT(p13, p14);
DigitalOut BT_CMD(p12);

MPU6050 imu(p9, p10);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

bool imu_available = false;

// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

bool setup_led(void)
{
    ledReset();
    return true;
}

bool setup_bt(void)
{
    BT.baud(115200);
    BT_CMD = 0;
    BT.printf("Bluetooth online!\n");
    return true;
}

bool setup_mpu6050(void)
{
    imu.reset();
    imu.debugSerial.baud(115200);
    wait_ms(5);

    imu.initialize();
    imu_available = imu.testConnection();

    imu.debugSerial.printf("IMU status...\t\t\t");
    imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");

    if (imu_available) {
        devStatus = imu.dmpInitialize();

        /*
         // supply your own gyro offsets here, scaled for min sensitivity
        mpu.setXGyroOffset(-61);
        mpu.setYGyroOffset(-127);
        mpu.setZGyroOffset(19);
        mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
        */

        if(!devStatus) {
            imu.setDMPEnabled(true);
            while (!imu.getIntDataReadyStatus());
            packetSize = imu.dmpGetFIFOPacketSize();
            dmpReady = true;
        } else {
            imu.debugSerial.printf("\tDMP setup failed!\n");
            return false;
        }

        imu.resetFIFO();
    } else {
        return false;
    }

    imu.debugSerial.printf("IMU setup routine done!");

    return dmpReady;
}