File content as of revision 2:b9b42ff80e9a:

#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
    0xC0, 0xBE, // 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 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_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 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 != 1) {
        return;
    }

    Direction new_direction;
    if (z == 1) {
        new_direction = UP;
    } else if (z == -1) {
        new_direction = DOWN;
    } else if (y == 1) {
        new_direction = LEFT;
    } else if (y == -1) {
        new_direction = RIGHT;
    } else if (x == 1) {
        new_direction = BACK;
    } else if (x == -1) {
        new_direction = FRONT;
    }

    if (direction == new_direction) {
        return;
    }

    direction = new_direction;

#if DEBUG
    log_direction();
#endif
    update_direction_characteristic();
}

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

void periodicCallback(void)
{
    update_cube_direction();
}

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();

    Ticker ticker;
    ticker.attach(periodicCallback, 1);

    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();
    }
}