File content as of revision 2:02175845b24c:

#include "mbed.h"
#include "MPU9250.h"
#include "ble/BLE.h"
#include "ButtonService.h"

// Serial comms
Serial pc(USBTX, USBRX);

// Sensor board library
MPU9250 mpu = MPU9250(p26, p27);

// Configuration
bool testMPUConnection = true;
bool doSensorInitialization = false;
bool printAccelerometer = false;
bool printGyroscope = false;

DigitalOut  led1(LED1);
InterruptIn button(BUTTON1);

const static char     DEVICE_NAME[] = "Terminator";
static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};

enum {
    RELEASED = 0,
    PRESSED,
    IDLE
};

static uint8_t buttonState = IDLE;

static ButtonService *buttonServicePtr;

void buttonPressedCallback(void)
{
    /* Note that the buttonPressedCallback() executes in interrupt context, so it is safer to access
     * BLE device API from the main thread. */
    buttonState = PRESSED;
}

void buttonReleasedCallback(void)
{
    /* Note that the buttonReleasedCallback() executes in interrupt context, so it is safer to access
     * BLE device API from the main thread. */
    buttonState = RELEASED;
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    BLE::Instance().gap().startAdvertising();
}

void periodicFunctionCallback(void)
{
    led1 = !led1; /* Do blinky on LED1 to indicate system aliveness. */
}

/**
 * This function is called when the ble initialization process has failled
 */
void onBleInitError(BLE &ble, ble_error_t error)
{
    /* Initialization error handling should go here */
}

/**
 * Callback triggered when the ble initialization process has finished
 */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);

    /* Setup primary service */
    buttonServicePtr = new ButtonService(ble, false /* initial value for button pressed */);

    /* setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
    ble.gap().startAdvertising();

}

int main ()
{
    // Turn the led
    led1 = 1;
    int16_t accelerometer[3] = {0,0,0};
    int16_t gyroscope[3] = {0,0,0};

    //Attach a function to be called by the Ticker, specifiying the interval in seconds.
    Ticker ticker;
    ticker.attach(periodicFunctionCallback, 1);
    button.fall(buttonPressedCallback);
    button.rise(buttonReleasedCallback);

    BLE &ble = BLE::Instance();
    ble.init(bleInitComplete);

    /* SpinWait for initialization to complete. This is necessary because the
    * BLE object is used in the main loop below. */
    while (ble.hasInitialized()  == false) {
        /* spin loop */
    }

    if (testMPUConnection) {
        uint8_t whoami = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
        pc.printf("I AM 0x%x\n\r", whoami);
        pc.printf("I SHOULD BE 0x71\n\r");
        wait(1);
    }

    if (doSensorInitialization) {
        // Initialise sensor board
        pc.printf("Initializing sensor\n\r");
        mpu.initMPU9250();
        pc.printf("Initialization finished\n\r");
        wait(1);
    }

    while(1) {

        //pc.printf("Starting to stream data");
        if(printAccelerometer && printGyroscope) {
            mpu.readAccelData(accelerometer);
            float ax = accelerometer[0] * 2.0 / 32768.0;
            float ay = accelerometer[1] * 2.0 / 32768.0;
            float az = accelerometer[2] * 2.0 / 32768.0;

            //pc.printf("Acelerometer information, AX, AY, AZ \n");
            //pc.printf("(%f, %f, %f)\n", ax,ay,az);

            float roll = float(atan2(ay, az) * 180/3.1416f);
            float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f);
            float yaw = atan(ax/-ay);

            //pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw);

            mpu.readGyroData(gyroscope);
            float gx = gyroscope[0] * 250.0 / 32768.0;
            float gy = gyroscope[1] * 250.0 / 32768.0;
            float gz = gyroscope[2] * 250.0 / 32768.0;


            pc.printf("(%f, %f, %f, %f, %f, %f)\n", roll, pitch, yaw, gx, gy, gz);


        }
        if (buttonState != IDLE) {
            //buttonServicePtr->updateButtonState(buttonState);
            //char myword[] = { 'H', 'e', 'l', 'l', 'o', '\0' };          
            uint8_t sensorValues[12] = {0};
            sensorValues[0] = 12;
            sensorValues[1] = 12;
            sensorValues[2] = 12;
            sensorValues[3] = 12;
            sensorValues[4] = 12;
            sensorValues[5] = 12;
            sensorValues[6] = 12;
            sensorValues[7] = 12;
            sensorValues[8] = 12;
            sensorValues[9] = 12;
            sensorValues[10] = 12;
            sensorValues[11] = 12;
            buttonServicePtr->updateButtonState(sensorValues);
            buttonState = IDLE;
        }
        ble.waitForEvent();

        //wait(0.3);
    }
}