File content as of revision 16:2c6432b37cce:

// A high prio thread reads at a rate of 1kHz from a MPU6050 gyro/acc meter's FIFO
// data packets of different size, whereas the lower prio ain thread the CPU time left.

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

//#include "DigitalOut.h"
//DigitalOut osci(p8);

volatile int g_disco=0;
volatile int g_len=0;
volatile int g_freq=100000;
const uint32_t i2cAdr = 0x68<<1;

static void config(I2CMasterRtos& i2c);

I2CMasterRtos i2c(p28, p27);

void highPrioCallBck(void const *args)
{
    //I2CDriver::osci2.write(0);
    const char reg= 0x74;
    static char result[64];
    //I2CDriver::osci2.write(1);
    // read from MPU600's fifo
    i2c.frequency(g_freq);
    uint32_t t1=us_ticker_read();
    //I2CDriver::osci2.write(0);
    int stat = i2c.read(i2cAdr, reg, result, g_len);
    uint32_t dt=us_ticker_read()-t1;
    if(stat!=0) {
        printf("\n%x %d %d %d\n",stat,g_freq,g_len,dt);
        exit(0);
    }
    int16_t val=((static_cast<int16_t>(result[0])<<8)|static_cast<int16_t>(result[1]));

    if(--g_disco>0)printf("val=%8d dt=%4dus\n",val,dt);
}

int doit()
{
    config(i2c);
    
    RtosTimer highPrioTicker(highPrioCallBck, osTimerPeriodic, (void *)0);

    Thread::wait(500);
    highPrioTicker.start(1);

#if defined(TARGET_LPC1768)
    const int nTest=7;
    const int freq[nTest]=  {1e5,   1e5,    1e5,   4e5,    4e5,    4e5,    4e5};
    const int len[nTest]=   {1,     4,      7,      1,     6,     12,      36};
#elif defined(TARGET_LPC11U24)
    const int nTest=5;
    const int freq[nTest]=  {1e5,   1e5,    4e5,   4e5,    4e5    };
    const int len[nTest]=   {1,     6,      1,      6,     32};
#endif
    for(int i=0; i<nTest; ++i) {
        g_freq = freq[i];
        g_len = len[i];
        printf("f=%d l=%d\n",g_freq,g_len);
        Thread::wait(500);
        //highPrioTicker.start(1);
        const uint32_t dt=1e6;
        uint32_t tStart = us_ticker_read();
        uint32_t tLast = tStart;
        uint32_t tAct = tStart;
        uint32_t tMe=0;
        do {  // loop an count consecutive µs ticker edges
            //osci.write(!osci.read());
            tAct=us_ticker_read();
            #if defined(TARGET_LPC1768)
            if(tAct==tLast+1)++tMe;
            #elif defined(TARGET_LPC11U24)
            uint32_t delta = tAct-tLast;
            if(delta<=2)tMe+=delta;  // on the 11U24 this loop takes a bit longer than 1µs (ISR ~3µs, task switch ~8µs)
            #endif
            tLast=tAct;
        } while(tAct-tStart<dt);
        //highPrioTicker.stop();
        // and calculate the duty cycle from this measurement
        printf("dc=%5.2f \n", 100.0*(float)tMe/dt);
        g_disco=5;
        while(g_disco>0);
    }
    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++]=0;// conf  reg // no ext frame sync / no dig low pass set to 1 => 8kHz 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++]=0xf<<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]);
}