File content as of revision 1:598008f00b35:

#include "mbed.h"
#include "MPU6050.h"

class MySerial :
    public SerialBase
{
public:
    MySerial(PinName txPin, PinName rxPin, int baud) :
    SerialBase(txPin, rxPin, baud)
    { }

    using SerialBase::_base_putc;
};

MySerial        serial(USBTX, USBRX, 460800);
MPU6050         mpu6050(0x68, AFS_2G, GFS_250DPS, I2C_SDA, I2C_SCL, NC);
Timer           mpu6050Timer;
Ticker          dispEventTicker;
volatile bool   dispEvent = false;

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void dispEventTick()
{
    dispEvent = true;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void print(int16_t val)
{
    if (val < 0) {
        serial._base_putc('-');
        val = -val;
    }

    uint16_t denom;
    uint8_t  digit;
    uint8_t  iter;

    if (val == 0) {
        serial._base_putc('0');
        return;
    }

    if (val < 10) {
        iter = 1;
        denom = 10;
    }
    else if (val < 100) {
        iter = 2;
        denom = 100;
    }
    else {
        iter = 3;
        denom = 1000;
    }

    for (int i = 0; i < iter; i++) {
        denom = denom / 10;
        digit = val / denom;
        val = val % denom;
        serial._base_putc(digit + '0');
    }
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
int main()
{
    uint32_t    mpu6050LastUpdate = 0;
    bool        mpu6050GainAdjusted = false;
    uint32_t    now;
    float       deltaT;

    printf("Starting...\r\n");

    mpu6050.init();
    mpu6050Timer.reset();
    mpu6050Timer.start();
#if MBED_MAJOR_VERSION > 5
    dispEventTicker.attach(dispEventTick, 2s);
#else
    dispEventTicker.attach(dispEventTick, 2.0f);
#endif

    while (true) {
        if (mpu6050.dataReady()) {
            mpu6050.accel();
            mpu6050.gyro();
            now = mpu6050Timer.read_us();
            mpu6050Timer.reset();
            deltaT = float(now) * 1e-6;   // integration time in seconds
            mpu6050.madgwickFilter(deltaT);
        }

        if (dispEvent) {
            dispEvent = false;

            int16_t  yaw = mpu6050.yaw();
            int16_t  pitch = mpu6050.pitch();
            int16_t  roll = mpu6050.roll();

            print(yaw);
            serial._base_putc(',');
            print(pitch);
            serial._base_putc(',');
            print(roll);
            serial._base_putc('\r');
            serial._base_putc('\n');
        }
    }
}