//Registers defined
#define cra    0x00
#define crb    0x01
#define mr     0x02
#define cxhr   0x03
#define cxlr   0x04
#define czhr   0x05
#define czlr   0x06
#define cyhr   0x07
#define cylr   0x08
#define statr  0x09        //Status register if you want to use

//Variables to use
int cxh,cxl,cyh,cyl,czh,czl;
float cxf,cyf,czf,xh;
signed int16 cx,cy,cz;

//Init I2C
void init_i2c(){
   output_float(pin_a1);
   output_float(pin_a2);
}

//Write a byte to a MPU6050 register
void wr_5883(int reg,int val){
   i2c_start();
   i2c_write(0x3c);
   i2c_write(reg);
   i2c_write(val);
   i2c_stop();
}

//Setup and configure compass
void hmc_init(){
  wr_5883(cra,0x70);
  wr_5883(crb,0xA0);
  wr_5883(mr,0);
  delay_ms(6);
}

//Read all raw values calculate degrees with scaling of 2.56 (see text for table) and heading
void rd_5883_vals(){
   delay_ms(70);           //Wait at least 67mS although serial comms is more
   i2c_start();
   i2c_write(0x3c);
   i2c_write(cxhr);
   i2c_start();
   i2c_write(0x3d); 
   cxh = i2c_read(); 
   cxl = i2c_read();       
   cyh = i2c_read(); 
   cyl = i2c_read();                  
   czh = i2c_read(); 
   czl = i2c_read(0); 
   i2c_stop();  

   //Make all 16 bit signed int,make it float and scale  
   cx=(cxh<<8)|cxl;
   cxf=(float)cx*2.56;
   
   cy=(cyh<<8)|cyl;
   cyf=(float)cy*2.56;
   
   cz=(czh<<8)|czl;
   czf=(float)cy*2.56;
  
   //Calculate heading info
   xh=atan2(cyf,cxf);
   
   //For accurate you need the magnetic declination of where you at.
   //I used Durban, SA, which is 25.2833W degrees and in radians 0.44127742462516
   //West is important as its then positive. East should be negative.
   //You can find yours at www.noaa.gov
   //Doing this on x heading only, so board should be flat in zeeplane!
   
   xh+=0.44128;            //Rounded as it drifts yearly in any case
   
   if(xh<0)xh+=(2*pi);
   if(xh>(2*pi))xh-=(2*pi);
   
   //Convert to degrees from radians
   cxf=xh*180/pi;
  
}