File content as of revision 0:449ee9595cf6:

#include "mbed.h"
#include "BLEDevice.h"
#include "nRF51822n.h"
#include "MPU6050.h"
#include <math.h> 

#define DEBUG 1

enum Direction {
    UP,
    DOWN,
    LEFT,
    RIGHT,
    FRONT,
    BACK,
    UNDEFINED
};

Serial pc(USBTX, USBRX);

BLEDevice ble;

const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;
const static uint8_t beaconPayload[] = {
    0x00, 0x4C, // Company identifier code (0x004C == Apple)
    0x02,       // ID
    0x15,       // length of the remaining payload
    0xE2, 0x0A, 0x39, 0xF4, 0x73, 0xF5, 0x4B, 0xC4, // UUID
    0xA1, 0x2F, 0x17, 0xD1, 0xAD, 0x07, 0xA9, 0x61,
    0x13, 0x37, // the major value to differenciate a location
    0xFA, 0xCE, // the minor value to differenciate a location
    0xC8        // 2's complement of the Tx power (-56dB)
};

extern GattService cube_service;
extern GattCharacteristic directionCharacteristic;
extern uint8_t direction_data[1];

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;

Direction direction = UNDEFINED;

void log_direction(void)
{
    switch(direction)
    {
        case UP:
            pc.printf("Direction UP\n");
            break;
            
        case DOWN:
            pc.printf("Direction DOWN\n");
            break;
            
        case LEFT:
            pc.printf("Direction LEFT\n");
            break;
            
        case RIGHT:
            pc.printf("Direction RIGHT\n");
            break;
            
        case BACK:
            pc.printf("Direction BACK\n");
            break;
            
        case FRONT:
            pc.printf("Direction FRONT\n");
            break;
            
        default:
            pc.printf("Direction UNSET\n");
            break;
    }
}

int16_t inline direction_if_exited(int16_t acceleration) {
    if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
        return 1;
    } else if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
        return -1;
    } else {
        return 0;
    }
}

void update_cube_direction(void)
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    int16_t x = direction_if_exited(ax);
    int16_t y = direction_if_exited(ay);
    int16_t z = direction_if_exited(az);
    
    int16_t sum = abs(x) + abs(y) + abs(z);
    if (sum == 0 || sum != 1) {
        return;
    }
    
    if (z == 1) {
        direction = UP;
    } else if (z == -1) {
        direction = DOWN;
    } else if (y == 1) {
        direction = LEFT;
    } else if (y == -1) {
        direction = RIGHT;
    } else if (x == 1) {
        direction = BACK;
    } else if (x == -1) {
        direction = FRONT;
    }
    
#if DEBUG
    log_direction();
#endif
}

void update_direction_characteristic(void)
{    
    direction_data[0] = direction;
    ble.updateCharacteristicValue(directionCharacteristic.getHandle(),
            direction_data,
            sizeof(direction_data));
#if DEBUG
    pc.printf("Updated gatt characteristic\n");
#endif
}

void disconnectionCallback(void)
{
    pc.printf("Disconnected!\n");
    pc.printf("Restarting the advertising process\n");
    ble.startAdvertising();
}

void setup_ble(void)
{
    ble.init();
    ble.onDisconnection(disconnectionCallback);

    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
                    beaconPayload, sizeof(beaconPayload));
                    
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.setAdvertisingInterval(160);

    ble.startAdvertising();
    
    ble.addService(cube_service);
    
    pc.printf("BLE set up and running\n");
}

int main()
{
    setup_ble();
    pc.printf("MPU6050 test startup:\n");

    mpu.initialize();
    pc.printf("TestConnection\n");
    
    if (mpu.testConnection())
    {
        pc.printf("MPU success\n");
    }
    else
    {
        pc.printf("MPU error\n");
    }

    while(1)
    {
        ble.waitForEvent();
        update_cube_direction();
        update_direction_characteristic();
    }
}