File content as of revision 4:303fb83498fd:

#include "Accelerometer.hpp"

//Compute inverse two's complement to obtain a signed value
//See page 21: https://www.gel.usherbrooke.ca/s5info/h17/doc/app1/file/MMA8452Q.pdf
//See: https://en.wikipedia.org/wiki/Two's_complement
//Turns out, the signed char does it for free
//We have to specify "signed char" because the "standard does not specify if plain char is signed or unsigned"
//http://stackoverflow.com/a/2054941/3212785
int raw_axis_data_to_int(signed char raw_axis_data)
{
    return raw_axis_data;
}

char get_axis_register(Axis axis)
{
    switch(axis)
    {
        case AXIS_X: return OUT_X_MSB_REGISTER;
        case AXIS_Y: return OUT_Y_MSB_REGISTER;
        case AXIS_Z: return OUT_Z_MSB_REGISTER;
        default: return AXIS_INVALID;
    }
}

bool is_almost_equal(double a, double b, double tolerance)
{
    double difference = std::abs(a-b);
    return (difference <= tolerance);
}

Accelerometer::Accelerometer(
    PinName sda_pin, 
    PinName scl_pin, 
    const int slave_address
    ) :
    device(sda_pin, scl_pin),
    slave_address(slave_address)
{
}

void Accelerometer::write_register(const char register_address, const char new_value)
{
    const int left_shifted_slave_address = slave_address << 1;
    char data[2];
    data[0] = register_address;
    data[1] = new_value;

    const int write_return = device.write(left_shifted_slave_address, data, 2);
    if(write_return < 0)
    {
        printf("Write error: I2C error");
    }
}

char Accelerometer::read_register(const char register_address)
{
    char result;
    const int left_shifted_slave_address = slave_address << 1;

    const int write_return = device.write(left_shifted_slave_address, &register_address, 1, true);
    if(write_return < 0)
    {
        printf("Write error: I2C error");
    }

    const int read_return = device.read(left_shifted_slave_address, &result, 1);
    if(read_return != 0)
    {
        printf("Read error: I2C error (nack)");
    }

    return result;
}

//axis_data must be an array of 6 bytes
void Accelerometer::read_all_axis(signed char* axis_data)
{
    for(int i = 0; i < NUMBER_OF_DATA_REGISTERS; ++i)
    {
        const char current_register = OUT_X_MSB_REGISTER + i;
        axis_data[i] = read_register(current_register);
    }
}

void Accelerometer::print_all_axis_data()
{
    signed char axis_data[NUMBER_OF_DATA_REGISTERS];
    for(int i=0; i<NUMBER_OF_DATA_REGISTERS; i++)
    {
        axis_data[i] = 0;
    }

    read_all_axis(axis_data);

    printf("Register content: ");
    for(int i=0; i<NUMBER_OF_DATA_REGISTERS; i++)
    {
        const int current_data = (int)(axis_data[i]);
        printf("%d, ", current_data);
    }
    printf("\r\n");
}

void Accelerometer::set_standby()
{
    const char previous_ctrl_reg1 = read_register(CTRL_REG1_REGISTER_ADDRESS);
    const char new_ctrl_reg1_value = previous_ctrl_reg1 & ~(0x01);
    write_register(CTRL_REG1_REGISTER_ADDRESS, new_ctrl_reg1_value);
}

void Accelerometer::set_active()
{
    const char previous_ctrl_reg1 = read_register(CTRL_REG1_REGISTER_ADDRESS);
    const char new_ctrl_reg1_value = previous_ctrl_reg1 | 0x01;
    write_register(CTRL_REG1_REGISTER_ADDRESS, new_ctrl_reg1_value);
}

void Accelerometer::init()
{
    set_active();
}

int Accelerometer::read_axis_data_8_bits(Axis axis)
{
    const char axis_register = get_axis_register(axis);
    const char register_value = read_register(axis_register);
    return raw_axis_data_to_int(register_value);
}