void read_full_xyz(int16_t * acc_X, int16_t * acc_Y, int16_t * acc_Z){
    int i;
    int16_t temp[3];
    if(g_I2C_Msg.Status != READING)
    {
        g_I2C_Msg.Data_count = 6;
        g_I2C_Msg.Dev_adx = MMA_ADDR;
        g_I2C_Msg.Reg_adx = REG_XHI;
        g_I2C_Msg.Command = READ;
    }
    //i2c_read_bytes(MMA_ADDR, REG_XHI, data, 6);
    if(g_I2C_Msg.Status == READ_COMPLETE)
    {

        for ( i=0; i<3; i++ ) {
            temp[i] = (int16_t) ((data[2*i]<<8) | data[2*i+1]);
        }

        // Align for 14 bits
        *acc_X = temp[0]/4;
        *acc_Y = temp[1]/4;
        *acc_Z = temp[2]/4;
    }
}