File content as of revision 7:7424c51ee942:

/*
APP1 S5 info
Author : lemy2301 and gagv2103
*/

#include "acceleroMMA8452Q.h"
#include "math.h"

#define CONVERT_FACTOR 5729.57795131 // 360/(2*PI) * 100

Accelero::Accelero(int frequency): i2cPort(p28, p27)
{
    init(frequency);
}

void Accelero::init(int frequency)
{
    //Set the frequency of the I2C port
    i2cPort.frequency(frequency);
    
    //Reset command
    char command[2];
    command[0] = 0x2B;
    command[1] = 0x40;
    i2cPort.write(ACCELERO_WRITE_ADRESS, command, 2, false);
    wait(0.1);
    
    //Request WHO_AM_I register
    char response = 0x00;
    i2cPort.write(ACCELERO_WRITE_ADRESS, &ACCELERO_REGISTER_WHO_AM_I, 1, true);
    i2cPort.read(ACCELERO_READ_ADRESS, &response, 1, false);
    
    //Turn on the led 4 is the device is properly connected
    if(response == ACCELERO_RESPONSE_WHO_AM_I)
    {
        DigitalOut led4(LED4);
        led4 = 1;
    }
    
    //Change the power mode
    command[0] = ACCELERO_REGISTER_CTRL_REG2;
    command[1] = 0b10;
    i2cPort.write(ACCELERO_WRITE_ADRESS, command, 2, false);
    
    //Put the accelerometer in active mode
    //Change the data rate selection to 6.25Hz
    command[0] = ACCELERO_REGISTER_CTRL_REG1;
    command[1] = (0b110 << 3) | 0b1;
    i2cPort.write(ACCELERO_WRITE_ADRESS, command, 2, false);
}

vector Accelero::getAccelVector()
{
    char data[6];
    i2cPort.write(ACCELERO_WRITE_ADRESS, &ACCELERO_REGISTER_OUT_X_MSB, 1, true);
    i2cPort.read(ACCELERO_READ_ADRESS,  data, 6, false);
    
    vector accelVector;
    
    //Transform data to be really signed
    int tempData;
    tempData = (data[0] << 4) + (data[1] >> 4);
    if(tempData >= 2048)
    {
        tempData |= 0xFFFFF000;
    }
    accelVector.x = tempData;
    
    tempData = (data[2] << 4) + (data[3] >> 4);
    if(tempData >= 2048)
    {
        tempData |= 0xFFFFF000;
    }
    accelVector.y = tempData;
    
    tempData = (data[4] << 4) + (data[5] >> 4);
    if(tempData >= 2048)
    {
        tempData |= 0xFFFFF000;
    }
    accelVector.z = tempData;
    
    return accelVector;
}
    
int Accelero::getAngle()
{
    vector accelVector = getAccelVector();
    
    int denominateur = accelVector.x*accelVector.x + accelVector.y*accelVector.y + accelVector.z*accelVector.z;
    
    float result = acos(abs(accelVector.z)/sqrtf(denominateur)) * CONVERT_FACTOR;
    
    return int(result);
}