Hier der Code der Funktion, die Ergänzung ist schnell hingepfuscht. Ich verwende als Winkel Radiant*1000, dh PI/2 ist hier 3141 um in weiteren Berechnungen die schnelleren Integervariablen verweden zu können.

Code:
static inline void Kompass()
{

unsigned short h,l;
short x,y,xx,yy,kompass_x;//,offset_x,offset_y;
double temp;

//offset_x=-125;offset_y=15;

    if(!(i2c_start(Adresse_Kompass)))    
            {
            i2c_write(2);      // register  
            i2c_rep_start(Adresse_Kompass+I2C_READ);      // restart        
            h=i2c_readAck();
            l=i2c_readAck();
            kompass_akt=uniq(l,h);    
            l=i2c_readAck();//4
            l=i2c_readAck();//5
            l=i2c_readAck();//6
            l=i2c_readAck();//7
            l=i2c_readAck();//8
            l=i2c_readAck();//9
            h=i2c_readAck();//10
            l=i2c_readAck();//11
            xx=uniq(l,h);
            h=i2c_readAck();//12
            l=i2c_readNak();//13    
            yy=uniq(l,h);
            i2c_stop();

            x=xx-(-125); // Offset Test
            y=yy-15; // Offset Test

            temp=1000*atan2(y,x); // für Kompasswinkel            

            kompass_x=(short)temp;
            if (kompass_x<0) kompass_x=kompass_x+6282;

   richtung_kom=(kompass_akt/10); // Umrechnen rad*1000        
            richtung_kom=(richtung_kom*35)/2;
// für Testauswertung
debug_eins=richtung_kom;
debug_zwei=kompass_x;

            debug_fuenf=xx;
            debug_sechs=yy;
            

            }       
        
        else richtung_kom= (-9999); // verwerfen wenn keine neuen Daten vorhanden


}