File content as of revision 4:40d23f4295f9:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "platform/mbed_thread.h"

// populate from databuffer queue raw samples data before sending over ble
// struct RawData samples_raw_data;
// int sampleIndex = 0;
// int ptr = 0;
// while(sampleIndex < mumberOffSamples)
//  status byte is index 0
//  samples_raw_data.green_led_cnt  = (databuf[ptr+1]  << 16)  + (databuf[ptr+2]  << 8)  + (databuf[ptr+3]);
//  samples_raw_data.ir_led_cnt     = (databuf[ptr+4]  << 16)  + (databuf[ptr+5]  << 8)  + (databuf[ptr+6]);
//  samples_raw_data.red_led_cnt    = (databuf[ptr+7]  << 16)  + (databuf[ptr+8]  << 8)  + (databuf[ptr+9]);
//  samples_raw_data.green2_led_cnt = (databuf[ptr+10] << 16)  + (databuf[ptr+11] << 8)  + (databuf[ptr12]);
//  samples_raw_data.x              = (databuf[ptr+19] << 8)   + (databuf[ptr+20]);
//  samples_raw_data.y              = (databuf[ptr+21] << 8)   + (databuf[ptr+22]);
//  samples_raw_data.z              = (databuf[ptr+23] << 8)   + (databuf[ptr+24]);
struct RawData
{
  uint32_t green_led_cnt;
  uint32_t ir_led_cnt;
  uint32_t red_led_cnt;
  uint32_t green2_led_cnt;
  int16_t x;
  int16_t y;
  int16_t z;
};

// Blinking rate in milliseconds
#define BLINKING_RATE_MS                                                    500

#define RST_PIN   P5_6
#define MFIO_PIN  P5_4

I2C i2c(P3_4, P3_5);

const int addr = 0xAA;//0x55;

int main()
{   
    i2c.frequency(400000);
    char cmd[4];
        
    // Initialise the digital pin LED1 as an output
    DigitalOut led(LED1);
    DigitalOut rst(RST_PIN);
    DigitalOut mfio(MFIO_PIN);
    
    mfio = 0;
    rst = 1;
    thread_sleep_for(10);
    rst = 0;
    thread_sleep_for(10);
    rst = 1;
    //thread_sleep_for(1500);
    thread_sleep_for(100);

    //scanI2C();

    while (true) {
        //Read out status (app or boot mode)
        cmd[0] = 0x02;
        cmd[1] = 0x00;
        i2c.write(addr, cmd, 2);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        cmd[1] = 0x00;
        i2c.read(addr, cmd, 2);
        printf("1: %x %x\n", cmd[0], cmd[1]);
    
        //Switch to APP mode
        cmd[0] = 0x01;
        cmd[1] = 0x00;
        cmd[2] = 0x00;
        i2c.write(addr, cmd, 3);
        thread_sleep_for(1500);
        
        //Switch off MFIO
        mfio = 0;
        thread_sleep_for(1); //Wait 300us
        
        //Read out status (app or boot mode)
        cmd[0] = 0x02;
        cmd[1] = 0x00;
        i2c.write(addr, cmd, 2);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        cmd[1] = 0x00;
        i2c.read(addr, cmd, 2);
        printf("2: %x %x\n", cmd[0], cmd[1]);

        //Sensor hub firmware nummer opvragen
        cmd[0] = 0xFF;
        cmd[1] = 0x03;
        i2c.write(addr, cmd, 2);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        cmd[1] = 0x00;
        cmd[2] = 0x00;
        cmd[3] = 0x00;
        i2c.read(addr, cmd, 4);
        printf("3: %d %d %d %d\n", cmd[0], cmd[1], cmd[2], cmd[3]);

        //Raw data mode
        cmd[0] = 0x10;
        cmd[1] = 0x00;
        cmd[2] = 0x03;

        i2c.write(addr, cmd, 3);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("4: %x\n", cmd[0]);
        
        //Interrupt threshold
        cmd[0] = 0x10;
        cmd[1] = 0x01;
        cmd[2] = 0x01;

        i2c.write(addr, cmd, 3);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("5: %x\n", cmd[0]);
        
        //Set sample report period
        cmd[0] = 0x10;
        cmd[1] = 0x02;
        cmd[2] = 0x01;

        i2c.write(addr, cmd, 3);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("6: %x\n", cmd[0]);
        
        //Enable accelerometer
        cmd[0] = 0x44;
        cmd[1] = 0x04;
        cmd[2] = 0x01;
        cmd[3] = 0x00; //0x01
        
        i2c.write(addr, cmd, 4);
        thread_sleep_for(20);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("7: %x\n", cmd[0]);
        
        //Read accelerometer WHO_AM_I register
        cmd[0] = 0x41;
        cmd[1] = 0x04;
        cmd[2] = 0x0F;   
             
        i2c.write(addr, cmd, 3);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        cmd[1] = 0x00;
        i2c.read(addr, cmd, 2);
        printf("8: %x %x\n", cmd[0], cmd[1]);
        
        
        
        printf("Start\n");



        //Enable AFE
        cmd[0] = 0x44;
        cmd[1] = 0x00;
        cmd[2] = 0x01; 
        //MAYBE 4
        
        i2c.write(addr, cmd, 3);
        thread_sleep_for(250);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("9: %x\n", cmd[0]);
        
        //Set 100Hz read samples
        cmd[0] = 0x40;
        cmd[1] = 0x00;
        cmd[2] = 0x12; 
        cmd[3] = 0x18; 
                
        i2c.write(addr, cmd, 4);
        thread_sleep_for(2);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("10: %x\n", cmd[0]);
        
        
        
        
        //get data
        struct RawData samples_raw_data;
        int sampleIndex = 0;
        char rawData[1024];
        uint8_t amount = 0;
        memset(rawData,0,sizeof(rawData));
        for(uint8_t i=0; i<250; i++)
        {
            //Get status sensor hub
            cmd[0] = 0x00;
            cmd[1] = 0x00;
            i2c.write(addr, cmd, 2);
            thread_sleep_for(2);
        
            cmd[0] = 0x00;
            cmd[1] = 0x00;
            i2c.read(addr, cmd, 2);
            printf("dat1: %x %x\n", cmd[0], cmd[1]);
            
            //Get number of samples in the fifo
            cmd[0] = 0x12;
            cmd[1] = 0x00;
            i2c.write(addr, cmd, 2);
            thread_sleep_for(2);
        
            cmd[0] = 0x00;
            cmd[1] = 0x00;
            i2c.read(addr, cmd, 2);
            amount = cmd[1];
            printf("dat2: %x %x\n", cmd[0], cmd[1]);
            
            //Get data from fifo
            cmd[0] = 0x12;
            cmd[1] = 0x01;
            i2c.write(addr, cmd, 2);
            thread_sleep_for(2);
            
            i2c.read(addr, rawData, 1+((36+6)*amount));
            
            printf("Satus: %x\n", rawData[0]);
            printf("Data: ");
            for(uint32_t j=1; j<1+((36+6)*amount); j+=42)
            {
                //%zu
                printf("%u, %d, %d, %d\n", (uint32_t)(0x00<<24 | rawData[j+1]<<16 | rawData[j+2]<<8 | rawData[j+3]), (rawData[j+37]<<8 | rawData[j+38]), (rawData[j+39]<<8 | rawData[j+40]), (rawData[j+41]<<8 | rawData[j+42]));
            }
                
            //thread_sleep_for(200); 
            thread_sleep_for(1);       
        }
        
        
        
        
        
        //Disable AFE
        cmd[0] = 0x44;
        cmd[1] = 0x00;
        cmd[2] = 0x00; 
        //MAYBE 4
        
        i2c.write(addr, cmd, 3);
        thread_sleep_for(250);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("11: %x\n", cmd[0]);
        
        
        
        printf("Stop\n");
        
        
        
        //Disable Accelerometer
        cmd[0] = 0x44;
        cmd[1] = 0x04;
        cmd[2] = 0x00; 
        //MAYBE 4
        
        i2c.write(addr, cmd, 3);
        thread_sleep_for(20);
        
        cmd[0] = 0x00;
        i2c.read(addr, cmd, 1);
        printf("12: %x\n", cmd[0]);

        /*rst =! rst;
        mfio =! mfio;*/
        thread_sleep_for(BLINKING_RATE_MS);
        led = !led;
    }
}