Calliope Mini Magnetsensor auslesen mit mbed

#include "MicroBit.h"
MicroBit uBit;
PinName sda = PinName(p20);
PinName scl = PinName(p19);
MicroBitI2C myi2c = MicroBitI2C(sda,scl);
 
 
// Mag hat Adress 0x10
// zum Schreiben addr|0
// zum Lesem addr|1
// Angelehnt an https://github.com/ControlEverythingCommunity/BMX055/blob/master/C/BMX055.c
// dort fehlt allerdings nach dem Software-Reset das Aktivieren des Mags
char reg[1];
int result[1];
char config[2];
char data[6] = {0};
 
int main() {
    uBit.init();   
    
    // soft reset 
    config[0] = 0x4B;
    config[1] = 0x83;
    result[0] = myi2c.write(0x10<<1,config,2,true);
    
    
     // mag enable(!)
    config[0] = 0x4B;
    config[1] = 0x01;
    
    result[0] = myi2c.write(0x10<<1,config,2,true);
    // normal mode
    config[0] = 0x4C;
    config[1] = 0x00;
    result[0] = myi2c.write(0x10<<1,config,2,true);
    
    // enable X,Y,Z- Axis
    config[0] = 0x4E;
    config[1] = 0x84;
    result[0] = myi2c.write(0x10<<1,config,2,true);
    
    // No. of Repetitions for X-Y Axis = 9(0x04)
    config[0] = 0x51;
    config[1] = 0x04;
    result[0] = myi2c.write(0x10<<1,config,2,true);
    
    // No. of Repetitions for Z-Axis = 15(0x0F)
    config[0] = 0x52;
    config[1] = 0x0F;
    result[0] = myi2c.write(0x10<<1,config,2,true);
    
    uBit.sleep(1000);   
   
    reg[0] = 0x42; 
    int xMag;
    int yMag;
    int zMag;
    for (int z = 1;z<=10; z++ ){
        result[0] = myi2c.write(0x10<<1,reg,1,true);
        result[0] = myi2c.read(0x21,data,6);
        //printf("reading: %d\r\n",  result[0]);
        for (int index = 0; index < 6; index++){
            printf("%d\n\r", data[index]);
        }
        xMag = (data[1] * 256 + (data[0] & 0xF8)) / 8;
        if(xMag > 4095)
        {
            xMag -= 8192;
        };
    
        yMag = (data[3] * 256 + (data[2] & 0xF8)) / 8;
        if(yMag > 4095)
        {
            yMag -= 8192;
        }
        zMag = (data[5] * 256 + (data[4] & 0xFE)) / 2;
        if(zMag > 16383)
        {
            zMag -= 32768;
        }  
        printf("data are %d\r\n %d\r\n %d\r\n", xMag,yMag,zMag);
   
        uBit.sleep(1000);
        }
        release_fiber();
      
    };