File content as of revision 7:bc915651d90e:

//#include <events/mbed_events.h>
#include <mbed.h>
#include "MPU6050.h"
#include <string>
#include <stdio.h>
#include <events/mbed_events.h>
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "MotorService.h"

#define N_128_BIT_SERVICE 2

static uint8_t UUID_128_LIST[N_128_BIT_SERVICE*UUID::LENGTH_OF_LONG_UUID];
static uint8_t uuid128ListPos = 0;
static uint8_t uuid128ListAddedServices = 0;

static MotorService* motorServicePtr;

DigitalOut indicatorLED(p25, 0); 
MPU6050 mpu6050;
Serial pc(USBTX,USBRX);


const static char     DEVICE_NAME[] = "Spike";
// static const uint16_t uuid16_list[] = {GattService::UUID_BATTERY_SERVICE};
static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);


void add128bitServiceToList(const uint8_t * service){
    uuid128ListAddedServices++;
    for(int i=UUID::LENGTH_OF_LONG_UUID-1; i>=0; i--){
        UUID_128_LIST[uuid128ListPos] = service[i];
        uuid128ListPos++;
    }
}
 
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
    BLE::Instance().gap().startAdvertising();
}

void connectionStatusIndicator(void) {
    BLE &ble = BLE::Instance();
    if (ble.gap().getState().connected) {
        indicatorLED = 1;
        //eventQueue.call(updateSensorValue);
    } else {
        indicatorLED = 0;
    }
}

void onBleInitError(BLE &ble, ble_error_t error) {
    /* Initialization error handling should go here */
}

void onDataWrittenCallback(const GattWriteCallbackParams *params) {

}

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);
    ble.gattServer().onDataWritten(onDataWrittenCallback);

    /* Setup GATT services */
    motorServicePtr = new MotorService(ble, 0, 0, 0);
    add128bitServiceToList(MOTOR_SERVICE_LONG_UUID);
    /* Setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    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().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                     (const uint8_t *)UUID_128_LIST, uuid128ListAddedServices*UUID::LENGTH_OF_LONG_UUID);
    
    ble.gap().startAdvertising();

    //printMacAddress();
 }
 
void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

int i = 0;

void updateAccelValues(){
 
    // mpu6050.readAccelData(accelData);
    // mpu6050.getAres();
    // float ax = accelData[0]*aRes  - accelBias[0];
    // float ay = accelData[1]*aRes  - accelBias[1];
    // float az = accelData[2]*aRes  - accelBias[2];
    // float accelValues[3] = {ax,ay,az};
    mpu6050.readGyroData(gyroData);
    mpu6050.getGres();
    float gx = gyroData[0]*gRes - gyroBias[0];
    float gy = gyroData[1]*gRes - gyroBias[1];
    float gz = gyroData[2]*gRes - gyroBias[2];
    float gyroValues[3] = {gx,gy,gz};
        
    pc.printf("%f, ",gx);
    pc.printf("%f, ",gy);
    pc.printf("%f\r\n",gz);

    BLE &ble = BLE::Instance();
    if (ble.gap().getState().connected) {
        // update char vals
        ble.gattServer().write(
            motorServicePtr->getValueHandleMotor1(),
            (const uint8_t*)&gx,
            4
        );
        ble.gattServer().write(
            motorServicePtr->getValueHandleMotor2(),
            (const uint8_t*)&gy,
            4
        );
        ble.gattServer().write(
            motorServicePtr->getValueHandleMotor3(),
            (const uint8_t*)&gz,
            4
        );
    } 
}

void blink(){
    indicatorLED = 1;
    wait(1);
    indicatorLED = 0;
}

int main()
{
    pc.baud(115200);
    pc.printf("START_1\n");

    mpu6050.calibrate(accelBias, gyroBias);
    mpu6050.initialize();
    blink();
    pc.printf("START_2\n");
    

    eventQueue.call_every(2000, connectionStatusIndicator);
    eventQueue.call_every(200, updateAccelValues);
 
    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);
 
    blink();
    eventQueue.dispatch_forever();
    return 0;
   
}

// int main()
// {
//     pc.baud(115200);
//     pc.printf("START_1\n");
//     mpu6050.calibrate(accelBias, gyroBias);
//     mpu6050.initialize();
//     pc.printf("START_2\n");
    
   
 
//  /////////////////////////////////////////////////////////SERIAL////////////////////////////////////////////
//     while(0){
//         indicatorLED = 0;
//         wait(1);
//    if(pc.readable()){
//        if(pc.getc()=='.'){
//             pc.putc(pc.getc());
//             indicatorLED=1;
//           wait(1);
//           }
//             }
  
//         }
       
//  /////////////////////////////////////////////////////////SERIAL/////////////////////////////////////////////
 
//  /////////////////////////////////////////////////////////MPU6050////////////////////////////////////////////
 
//     while(0){
//          pc.printf("i am %d\n",mpu6050.getDeviceID());
//          wait(1);
//         }
       
//  /////////////////////////////////////////////////////////MPU6050/////////////////////////////////////////////
 
 
 
//  //////////////////////////////////////////////////////////////GET_DATA////////////////////////////////////////////
//     while(1){
 
//     mpu6050.readAccelData(accelData);
//     mpu6050.getAres();
//     ax = accelData[0]*aRes  - accelBias[0];
//     ay = accelData[1]*aRes  - accelBias[1];
//     az = accelData[2]*aRes  - accelBias[2];
//     float accelValues[3] = {ax,ay,az};
//     mpu6050.readGyroData(gyroData);
//     mpu6050.getGres();
//     gx = gyroData[0]*gRes - gyroBias[0];
//     gy = gyroData[1]*gRes - gyroBias[1];
//     gz = gyroData[2]*gRes - gyroBias[2];
//     float gyroValues[3] = {gx,gy,gz};
        
//     if(pc.readable())
//     {
//         if(pc.getc()=='.'){
            
//             pc.printf("%f, ",gx);
//             pc.printf("%f, ",gy);
//             pc.printf("%f",gz);
           
//         }

//     }
    
//     }
// }