File content as of revision 19:f5aa0ce5546b:

#include "Accelerometer.hpp"
#include "Utility.hpp"

#include <cmath>

namespace accelerometer
{
    //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;
        }
    }
     
    double g_force_from_int_axis_data(const int axis_data)
    {
        return (double)axis_data / 64.0;
    }
    
    //Z axis is perpendicular to the horizontal plane, towards the floor when the accelerometer is flat
    //Therefore, 
    // - if the Z force is +1g, the accelerometer is flat
    // - if the Z force is -1g, the accelerometer is upside down
    // - if the Z force is 0g, the accelerometer 90 degree from the horizontal
    //cos(theta) = Z force in g
    double angle_from_int_axis_data(const int axis_data)
    {
        const double z_g_force = g_force_from_int_axis_data(axis_data);
        const double absolute_z_g_force = std::fabs(z_g_force);
        if(absolute_z_g_force > 1.0)
        {
            return 0.0;
        }
        
        const double angle_radian = std::acos(absolute_z_g_force);
        return utility::degree_from_radian(angle_radian);
    }
    
    Accelerometer::Accelerometer(
        PinName sda_pin, 
        PinName scl_pin, 
        const int filter_size,
        const int slave_address
        ) :
        device(sda_pin, scl_pin),
        filter(filter_size),
        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);
    }
    
    double Accelerometer::get_angle_from_horizontal()
    {
        const int z_axis_data = read_axis_data_8_bits(AXIS_Z);
        const int filtered_z_axis_data = this->filter.calculate(z_axis_data);
        return angle_from_int_axis_data(filtered_z_axis_data);
    }
}