File content as of revision 7:04824382eafb:

#include "mbed.h"
#include "rtos.h"
#include "I2CMasterRtos.h"
#include "stdint.h"

const int dataReadySig = 1<<5;
osThreadId mainThreadID = 0;
char data[64];
int16_t fifo[16];
const int i2cAdr = 0x68<<1;
int fifoAdr = 0x72;

//Serial pc(USBTX, USBRX);

void configMPU6050(I2CMasterRtos& i2c);
void config(I2CMasterRtos& i2c);


void dataReadyIsr()
{
    osSignalSet(mainThreadID, dataReadySig);
}

void readModWrite(I2CMasterRtos& i2c, uint8_t reg, uint8_t dta)
{
    
    char rd1;
    int rStat1 = i2c.read(i2cAdr, reg, &rd1, 1);
    char data[2];
    data[0]=(char)reg;
    data[1]=(char)dta;
    char rd2;
    int wStat = i2c.write(i2cAdr, data, 2);
    osDelay(500);
    int rStat2 = i2c.read(i2cAdr, reg, &rd2, 1);
    printf("%2d%2d%2d  %2x <- %2x  => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2);
}


int doit()
{
    //pc.baud(115200);
    mainThreadID = osThreadGetId();

    I2CMasterRtos i2c(p28, p27,400000);
    osDelay(500);

    printf("Initialize ... \n");
    config(i2c);

    printf("Action!\n");

    InterruptIn dataReadyIrq(p8);
    dataReadyIrq.mode(PullNone);
    dataReadyIrq.rise(&dataReadyIsr);

    /*
    data[0]=0x6a;   // pwr 1 reg
    data[1]=(1<<6)|(1<<2); // fifo on
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x38;   // irq conf  reg
    data[1]=1;      // irq on data ready
    i2c.write(i2cAdr,data,2,1);
    */
    //fifoAdr = 0x3b;
    char devNull;
    while(1) {
        osSignalWait(dataReadySig, 1000); // osWaitForever
        i2c.read(i2cAdr,fifoAdr,data,2);
        i2c.read(i2cAdr,fifoAdr+2,data+2,12);
        i2c.read(i2cAdr,0x3a,&devNull,1);
        for(int i=0; i<7; i++) {
            fifo[i] = (data[2*i]<<8) | data[2*i+1];
            printf("%8d",fifo[i]);
        }
        printf("     %x\n",devNull);

    }
    return 0;
}

static void config(I2CMasterRtos& i2c)
{
    uint8_t ncfg=32;
    uint8_t regs[ncfg];
    uint8_t vals[ncfg];
    int cnt=0;
    regs[cnt]=0x6b;
    vals[cnt++]=(1<<7); // pwr 1 reg //: device reset
    regs[cnt]=0x6b;
    vals[cnt++]=1; // pwr 1 reg // clock from x gyro all pwr sav modes off
    regs[cnt]=0x19;
    vals[cnt++]=199;  // sample rate divider reg  // sapmle rate = gyro rate / (1+x)
    regs[cnt]=0x1a;
    vals[cnt++]=1;// conf  reg // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF
    regs[cnt]=0x1b;
    vals[cnt++]=0;// gyro conf  reg // no test mode and gyro range 250°/s
    regs[cnt]=0x1c;
    vals[cnt++]=0;// accl conf  reg // no test mode and accl range 2g
    regs[cnt]=0x23;
    vals[cnt++]=0xf<<3;// fifo conf  reg // accl + all gyro -> fifo
    regs[cnt]=0x37;
    vals[cnt++]=(0<<7)|(0<<6)|(0<<5)|(0<<4); // irq conf  reg // act high | 0:pupu 1:opnDrn| pulse | clear on any read
    regs[cnt]=0x38;
    vals[cnt++]=1|(1<<4); // irq conf  reg // irq on data ready
    regs[cnt]=0x6a;
    vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset
    regs[cnt]=0x6a;
    vals[cnt++]=(1<<6); // pwr 1 reg // fifo on

    /*
    readModWrite(i2c, regs[0], vals[0]);
    char reset=0xff;
    while(reset&(1<<7)) {
        osDelay(100);
        i2c.read(i2cAdr,0x6b,&reset,1,1);
    }
    */
    for(int i=0; i<cnt; i++)
        readModWrite(i2c, regs[i], vals[i]);
}

static void configMPU6050(I2CMasterRtos& i2c)
{

    data[0]=0x6b;   // pwr 1 reg
    data[1]=1<<7;   // device reset
    i2c.write(i2cAdr,data,2,1);
    char reset=0xff;
    while(reset&(1<<7)) {
        osDelay(100);
        i2c.read(i2cAdr,0x6b,&reset,1,1);
    }

    data[0]=0x19;   // sample rate divider reg
    data[1]=99;    // sapmle rate = gyro rate / (1+x)
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x1a;   // conf  reg
    data[1]=1;      // no ext frame sync / dig low pass set to 1 => 1kHz Sampling with ~200Hz bandwidth DLPF
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x1b;   // gyro conf  reg
    data[1]=0;      // no test mode and gyro range 250°/s
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x1c;   // accl conf  reg
    data[1]=0;      // no test mode and accl range 2g
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x23;   // fifo conf  reg
    data[1]=0xf<<3; // accl + all gyro -> fifo
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x37;   // irq conf  reg
    data[1]=(1<<7)|(0<<6)|(0<<5)|(1<<4); // act high | pupu | pulse | clear on any read
    i2c.write(i2cAdr,data,2,1);

    /*
    data[0]=0x38;   // irq conf  reg
    data[1]=1;      // irq on data ready
    i2c.write(i2cAdr,data,2,1);

    data[0]=0x6a;   // pwr 1 reg
    data[1]=(1<<6); // fifo on
    i2c.write(i2cAdr,data,2,1);
    */
    data[0]=0x6b;   // pwr 1 reg
    data[1]=1;      // clock from x gyro all pwr sav modes off
    i2c.write(i2cAdr,data,2,1);
}