File content as of revision 16:2c6432b37cce:

// several threads try to read concurrently from a MPU6050 gyro/acc meter
// via the same globally defined i2c driver interface

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

const uint32_t i2cAdr = 0x68<<1;
const char reg= 0x3b;  // accelerometer x,y,z
volatile osThreadId i2cDrvThrdID[2];

I2CMasterRtos g_i2c(p28, p27, 400000);

static void config(I2CMasterRtos& i2c);

void highPrioCallBck(void const *args)
{
    osSignalSet(i2cDrvThrdID[1], 1<<5);
    osSignalSet(i2cDrvThrdID[0], 1<<5);
}

void highPrioThreadFun(void const *args)
{
    int thrdID = (int)args;
    i2cDrvThrdID[thrdID] = Thread::gettid();

    char result[64];
    while(true) {
        Thread::signal_wait(1<<5,osWaitForever);
        g_i2c.lock();
        g_i2c.read(i2cAdr, reg, result, 3*2);
        printf("%s prio thread has read from MPU650:", (thrdID==0?"high       ":"even higher"));
        for(int i=0; i<3; i++) {
            int16_t acc=((static_cast<int16_t>(result[i*2])<<8)|static_cast<int16_t>(result[i*2+1]));
            printf("%7i",acc);
        }
        printf("\n");
        g_i2c.unlock();
    }
}

int doit()
{
    I2CMasterRtos i2c(p28, p27, 100000);
    config(i2c);

    Thread highPrioThread(highPrioThreadFun, 0, osPriorityAboveNormal);
    Thread evenHigherPrioThread(highPrioThreadFun, (void*)1, osPriorityHigh);
    RtosTimer highPrioTicker(highPrioCallBck, osTimerPeriodic, (void *)0);

    Thread::wait(1000);
    highPrioTicker.start(503);

    char result[64];
    for(int i=0; i<100; ++i) {
        i2c.read(i2cAdr, reg, result, 3*2);
        printf("normal      prio thread has read from MPU650:");
        for(int i=0; i<3; i++) {
            int16_t acc=((static_cast<int16_t>(result[i*2])<<8)|static_cast<int16_t>(result[i*2+1]));
            printf("%7i",acc);
        }
        printf("\n");
        Thread::wait(100);
    }
    return 0;
}

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(100);
    int rStat2 = i2c.read(i2cAdr, reg, &rd2,1);
    printf("(%3x%3x%3x)  %2x <- %2x  => %2x -> %2x \n", rStat1, wStat, rStat2, reg, dta, rd1, rd2);
}

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++]=0;  // sample rate divider reg  // sapmle rate = gyro rate / (1+x)
    regs[cnt]=0x1a;
    vals[cnt++]=0x01;// conf  reg // no ext frame sync / no dig low pass set to 1 => 1kHz Sampling
    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++]=0x1f<<3;// fifo conf  reg // accl + all gyro -> fifo
    //regs[cnt]=0x6a;
    //vals[cnt++]=(1<<2); // pwr 1 reg // fifo reset
    //regs[cnt]=0x6a;
    //vals[cnt++]=(1<<6); // pwr 1 reg // fifo on

    for(int i=0; i<cnt; i++)
        readModWrite(i2c, regs[i], vals[i]);
}