LSM303 accelerometer on RaspberryPi

After getting my new LSM303 accelerometer, I got my great electronics tec. Daviv Shtibelman to wire it up for RPI.
Connecting LSM303 pins 1 (SCL), 2 (SDA), 6 (GND) and 8 (V3V) to pins 5 (SCL), 3 (SDA), 6 (GND) and 1 (V3V) on the Pi board.
On my ArchlinuxARM OS I had to install i2c-tools:
> pacman -S i2c-tools
And add i2c modules:
> echo i2c-dev > /etc/modules-load.d/ i2c.conf
> echo i2c-bcm2708 >> /etc/modules-load.d/ i2c.conf
> reboot

now to check it works:
> i2cdetect -y 1
got a message like this:
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- 19 -- -- -- -- 1e --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --

Now, following this, I test it and it works. Now I'm up for creating my own code.

Made some changes to the code:

main.cpp

#include"LSM303DLHC.h"
#include<stdio.h>
#include <unistd.h>


/*
   getHeading() calculates a tilt-compensated heading.
   A float between 0 and 360 degrees is returned. You need
   to pass this function both a magneto and acceleration array.
 
   Headings are calculated as specified in AN3192:
   http://www.sparkfun.com/datasheets/Sensors/Magneto/Tilt%20Compensated%20Compass.pdf
 
*/

float getHeading(LSM303& lsm303dlhc);

#define PI 3.141592654


int main(void)
{
    uint8_t bajt;
    const char *fileN = "/dev/i2c-1";
    LSM303 lsm303dlhc(fileN);

    lsm303dlhc.enalbe();

    while(1)
    {
        lsm303dlhc.readAccelerationRaw();
        lsm303dlhc.readMagnetometerRaw();
        printf("acc [m/s^2]: \e[27;1;31m %f \e[m \e[27;1;32m %f \e[m \e[27;1;34m %f \e[m mag:  \e[27;1;31m %d \e[m  \e[27;1;32m %d \e[m \e[27;1;34m %d\e[m %fdeg\n",
               (int16_t)lsm303dlhc.acc_x_raw*0.00957,(int16_t)lsm303dlhc.acc_y_raw*0.00957,-(int16_t)lsm303dlhc.acc_z_raw*0.00957,
               (int16_t)lsm303dlhc.mag_x_raw, (int16_t)lsm303dlhc.mag_y_raw, (int16_t)lsm303dlhc.mag_z_raw,getHeading(lsm303dlhc));
        usleep(10);
       }

    }


float getHeading(LSM303& lsm303dlhc)
{
  float heading,pitch,roll,xh,yh,zh;
  // see section 1.2 in app note AN3192
  int magValue[3];
  float accelValue[3];
  magValue[0] = (int16_t)lsm303dlhc.mag_x_raw;
  magValue[1] = (int16_t)lsm303dlhc.mag_y_raw;
  magValue[2] = (int16_t)lsm303dlhc.mag_z_raw;
  accelValue[0] = (int16_t)lsm303dlhc.acc_x_raw*0.000976531;
  accelValue[1] = (int16_t)lsm303dlhc.acc_y_raw*0.000976531;
  accelValue[2] = -(int16_t)lsm303dlhc.acc_z_raw*0.000976531;

    // see appendix A in app note AN3192
  pitch = asin(-accelValue[0]);
  roll = asin(accelValue[1]/cos(pitch));

  xh = magValue[0] * cos(pitch) + magValue[2] * sin(pitch);
  yh = magValue[0] * sin(roll) * sin(pitch) + magValue[1] * cos(roll) - magValue[2] * sin(roll) * cos(pitch);
  zh = -magValue[0] * cos(roll) * sin(pitch) + magValue[1] * sin(roll) + magValue[2] * cos(roll) * cos(pitch);

  heading = 180*atan2(yh,xh)/PI;

  if (heading <0)
    heading += 360;

  return heading;
}

LSM303DLHC.cpp:

#include"LSM303DLHC.h"
#include<math.h>
#include<stdio.h>

/*Conection to Raspberry PI:
 LSM303     Raspberry PI
 VDD    ->  3V3(PIN 1)
 SDA    ->  SDA(PIN 3)
 SCL    ->  SCL(PIN 5)
 GND    ->  GND(PIN 6)
*/

#define LSM303DLHC_MAG_ADDRESS            (0x3C >> 1)
#define LSM303DLHC_ACC_ADDRESS            (0x32 >> 1)

LSM303::LSM303(const char * i2cDeviceName) : i2c_lsm303(i2cDeviceName)
{

}

uint8_t LSM303::readAccRegister(uint8_t regAddr)
{
    i2c_lsm303.addrSet(LSM303DLHC_ACC_ADDRESS);
    return i2c_lsm303.readByte(regAddr);
}

uint8_t LSM303::readMagRegister(uint8_t regAddr)
{
    i2c_lsm303.addrSet(LSM303DLHC_MAG_ADDRESS);
    return i2c_lsm303.readByte(regAddr);
}

void LSM303::writeAccRegister(uint8_t regAddr,uint8_t byte)
{
    i2c_lsm303.addrSet(LSM303DLHC_ACC_ADDRESS);
    i2c_lsm303.writeByte(regAddr, byte);

}

void LSM303::writeMagRegister(uint8_t regAddr, uint8_t byte)
{
    i2c_lsm303.addrSet(LSM303DLHC_MAG_ADDRESS);
    i2c_lsm303.writeByte(regAddr, byte);

}

void LSM303::enalbe(void)
{
   writeAccRegister(LSM303_CTRL_REG1, 0b10010111);
   writeAccRegister(LSM303_CTRL_REG4, 0b00001000);

   writeMagRegister(LSM303_MR_REG, 0x00);
}

void LSM303::readAccelerationRaw(void)
{
    uint8_t block[6];

    i2c_lsm303.addrSet(LSM303DLHC_ACC_ADDRESS);

    i2c_lsm303.readBlock(0x80 | LSM303_OUT_X_L_A, sizeof(block), block);
    acc_x_raw = (int16_t)(block[0] | (block[1] << 8)) >> 4;
    acc_y_raw = (int16_t)(block[2] | block[3] << 8) >> 4;
    acc_z_raw = (int16_t)(block[4] | block[5] << 8) >> 4;

}

void LSM303::readMagnetometerRaw(void)
{
    uint8_t block[6];

    i2c_lsm303.addrSet(LSM303DLHC_MAG_ADDRESS);
    i2c_lsm303.readBlock(0x80 | LSM303_OUT_X_H_M, sizeof(block), block);

    mag_x_raw = (int16_t)(block[1] | block[0] << 8);
    mag_y_raw = (int16_t)(block[5] | block[4] << 8);
    mag_z_raw = (int16_t)(block[3] | block[2] << 8);

}

void LSM303::readAcceleration(void)
{
    readAccelerationRaw();
}