File content as of revision 3:f14ad02770e4:

#include "HMC5883L.h"
static I2C i2c(p4, p2);         // setup i2c (SDA,SCL)  

 uint16_t  mode;
float    declination_offset_radians;

void HMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data){
    char data_write[2];
    data_write[0]=regAddress;           // I2C sends MSB first. Namely  >>|regAddress|>>|data|
    data_write[1]=data;
    i2c.write(address,data_write,2,0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
}

char HMC5883L::readByte(uint8_t address, uint8_t regAddress)
{
    char data_read[1];                   // will store the register data    
    char data_write[1];
    data_write[0]=regAddress;
    i2c.write(address,data_write,1,1);   // repeated = true
    i2c.read(address,data_read,1,0);     // read the data and stop
    return data_read[0];
} 

void HMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
{
    char data[10],data_write[1];  
    data_write[0]=regAddress;      
    i2c.write(address,data_write,1,1);
    i2c.read(address,data,byteNum,0);
    for(int i=0;i<byteNum;i++)          // equate the addresses
        dest[i]=data[i];
}

void HMC5883L::init(){
    declination_offset_radians =  0 - (( 54 + (1/60 * 4) ) * (M_PI / 180));
    mode = COMPASS_CONTINUOUS | COMPASS_SCALE_130 | COMPASS_VERTICAL_X_EAST;  
    writeByte(HMC5883L_ADDRESS, CONFIG_A, 0x78);  // Number of samples averaged: 8, Data output rate: 75 Hz
    writeByte(HMC5883L_ADDRESS, MODE,     mode & 0x03);  // Continuous-Measurement Mode
    wait_ms(10);
}
void HMC5883L::setScale( uint16_t scale ){
  // Scale is the bits marked S in mode
  //    xxxxxxxxxxxSSSMM  
  mode = (mode & ~0x1C) | (scale & 0x1C);
  writeByte(HMC5883L_ADDRESS,CONFIG_B, (( mode >> 2 ) & 0x07) << 5);
}

/** Set the orientation to one of COMPASS_HORIZONTAL_X_NORTH 
 * through COMPASS_VERTICAL_Y_WEST
 *  
 */

void HMC5883L::setOrientation( uint16_t orientation )
{
  // Orientation is the bits marked XXXYYYZZZ in mode
  //    xxXXXYYYZZZxxxxx
  mode = (mode & ~0x3FE0) | (orientation & 0x3FE0);    
}

float s_mag_north, s_mag_west;
double HMC5883L::getHeading(){
      
    float sample[3];
    readMagData(sample);
  
  
  
  float heading;    
  
  // Determine which of the Axes to use for North and West (when compass is "pointing" north)
  float mag_north, mag_west = -100000;

 switch((mode >> 5) & 0x07 )
  {
    case COMPASS_NORTH: mag_north = sample[2]; break;
    case COMPASS_SOUTH: mag_north = 0-sample[2]; break;
    case COMPASS_WEST:  mag_west  = sample[2]; break;
    case COMPASS_EAST:  mag_west  = 0-sample[2]; break;
      
    // Don't care
    case COMPASS_UP:
    case COMPASS_DOWN:
     break;
  }
  
  // Y = bits 3 - 5
  switch(((mode >> 5) >> 3) & 0x07 )
  {
    case COMPASS_NORTH: mag_north = sample[1];  break;
    case COMPASS_SOUTH: mag_north = 0-sample[1]; ;  break;
    case COMPASS_WEST:  mag_west  = sample[1];  break;
    case COMPASS_EAST:  mag_west  = 0-sample[1];  break;
      
    // Don't care
    case COMPASS_UP:
    case COMPASS_DOWN:
     break;
  }
  
  // X = bits 6 - 8
  switch(((mode >> 5) >> 6) & 0x07 )
  {
    case COMPASS_NORTH: mag_north = sample[0]; break;
    case COMPASS_SOUTH: mag_north = 0-sample[0]; break;
    case COMPASS_WEST:  mag_west  = sample[0]; break;
    case COMPASS_EAST:  mag_west  = 0-sample[0]; break;
      
    // Don't care
    case COMPASS_UP:
    case COMPASS_DOWN:
     break;
  }
    if(s_mag_north == -100000) {
        s_mag_north = mag_north;
    }
    if(s_mag_west == -100000) {
        s_mag_west = mag_west;
    }
  // calculate heading from the north and west magnetic axes
  heading = atan2(mag_west - s_mag_west, mag_north- s_mag_north);
  // Adjust the heading by the declination
  heading += declination_offset_radians;
  
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*M_PI;
    
 // Check for wrap due to addition of declination.
  if(heading > 2*M_PI)
    heading -= 2*M_PI;
    
  // Convert radians to degrees for readability.
  heading =  heading * 180/M_PI; 
  return heading ;
}


void HMC5883L::readMagData(float* dest){
    uint8_t rawData[6]; // x,y,z mag data
    
    /* Read six raw data registers sequentially and write them into data array */
    readBytes(HMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]); 
    
    /* Turn the MSB LSB into signed 16-bit value */
    dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // MAG_XOUT
    dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // MAG_ZOUT
    dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // MAG_YOUT 
    

}