File content as of revision 81:dded8c042cca:

#define END_OF_JSON          0xFE
#define VERBOSE_MODE            1  // Habilita la generacion de logs por el puerto USB      

#include "mbed.h"
#include "picojson.h"
#include "stats_report.h"

/* Librerías para BLE - Servicio GATT */
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "TOFService.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"

const static char     DEVICE_NAME[]        = "BLE_CONTROLLER";
static const uint16_t uuid16_list[]        = {TOFService::CUSTOM_TOF_SERVICE_UUID};

bool serviceDiscovered     = false;
char finalCharacterJSON    = END_OF_JSON;

typedef struct {
    string           MAC1;       /* Dirección MAC1*/
    string           MAC2;       /* Dirección MAC2*/
    double            ToF;       /* Tiempo de vuelo*/
} BLEdata_t;

typedef struct {
    string           hazardousDevice;       /* MAC dispositvo peligroso */
    string            affectedDevice;       /* MAC dispositivo afectado */
    uint8_t                     type;       /* Tipo de evento           */
} JSONdata_t;

DigitalOut led3Test(LED3);
DigitalOut led4BLE(LED4);
Serial pcSerial(USBTX, USBRX); // Abrimos conexión serial con el puerto USB

TOFService* tofService;

EventQueue eventQueue;
Queue<JSONdata_t, 32> JSONqueue;
Queue<BLEdata_t, 32> BLEqueue;
Mail<BLEdata_t, 16> BLEMail;

Thread threadLED(osPriorityAboveNormal1, 400);
Thread threadSerial(osPriorityAboveNormal2, 2500);
Thread threadBLE(osPriorityRealtime3, 2500);
Thread threadCalculateEvent(osPriorityRealtime2, 1500);

template<typename arg>
void printLog(const char * log, arg data) {
    if(VERBOSE_MODE) printf(log, data);
}

void printLog(const char * log) {
    if(VERBOSE_MODE) printf(log);
}

/**
 * Thread encargado de parpadear un LED continuamente
 */
void blinkLED3() {
    while(true) {
        led3Test = !led3Test;
        wait(0.8);
    }
}

/**
 * Método encargado de enviar un string por el puerto serie char a char
 */
void sendCharArrayToSerial(char const *array, Serial *serial) {
    uint32_t i = 0;
    while(array[i] != '\0') {
        serial->putc(array[i]);
        i++;
    }
    serial->putc('\0');
}

/**
 * Thread encargado de calcular el tipo de evento a enviar
 */
void calculateEvent() {
    while(true) {
        printLog("Leo evento de BLEMail\r\n");
        osEvent evt = BLEMail.get();
        printLog("Evento leido de BLEMail\r\n");
        if (evt.status == osEventMail) {
            BLEdata_t *dataIn = (BLEdata_t*) evt.value.p;
            printLog("ToF: %s\r\n", dataIn->ToF);
            printLog("Se obtiene puntero de BLEdata\r\n");
            
            JSONdata_t event;
            printLog("Se declara objeto JSONdata_t\r\n");
            if(dataIn->ToF > 1.0) {
                printLog("Evento de tipo 1\r\n");
                event.hazardousDevice = dataIn->MAC1;
                event.affectedDevice  = dataIn->MAC2;
                event.type            = 1; 
            } else { // Keep Alive
                event.type            = 0;
                event.hazardousDevice = dataIn->MAC1;
            }
            BLEMail.free(dataIn);
            printLog("Se va a escribir en la cola de JSON\r\n");
            JSONqueue.put(&event);
            printLog("Evento enviado a JSON\r\n");
        }
    }
}

/**
 * Thread encargado de enviar JSONs por el puerto serie
 */
void sendJsonOverSerial() {
    char tmp[512]; // Vble auxiliar para el tratamiento de cadenas de char.
    string str;
    while(true) {
        // Esperamos a un mensaje en la cola
        picojson::object json;
        picojson::object event;
        picojson::object extraInfo;
        
        osEvent evt = JSONqueue.get();
        if (evt.status == osEventMessage) {
            JSONdata_t *data = (JSONdata_t*) evt.value.p;

            int id = rand() % 1000;
            event["idEvent"] = picojson::value((double) id);
            event["type"] = picojson::value((double) data->type);
            //str = "E9:ED:F4:AC:BF:8E";
            extraInfo["hazardousDevice"] = picojson::value(data->hazardousDevice);
            //str = "D5:62:12:BF:B8:45";
            
            if(data->type != 0) extraInfo["affectedDevice"] = picojson::value(data->affectedDevice);
            event["extraInfo"] = picojson::value(extraInfo);
            json["Event"] = picojson::value(event);
            
            str = picojson::value(json).serialize();
                       
            // Convertimos el string a char *
            strncpy(tmp, str.c_str(), sizeof(tmp));
            strncat(tmp, &finalCharacterJSON, sizeof(finalCharacterJSON)); // Añadimos el caracter al final
            tmp[sizeof(tmp) - 1] = 0;
            
            sendCharArrayToSerial(tmp, &pcSerial);
        }
    }
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) {
    printLog("Desconectado. Se comienza la fase de Advertising de nuevo\r\n");
    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
}

void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
    printLog("Conectado al servidor\r\n");    
}

void parseRawJSONToQueue(string JSONsource) {
    picojson::value v;
    string err, MAC1, MAC2;
    BLEdata_t *data = BLEMail.alloc();;
    
    picojson::parse(v, JSONsource.c_str(), JSONsource.c_str() + strlen(JSONsource.c_str()), &err);
    printLog("res error? %s\r\n", err);
    
    printLog("ToF %f\r\n", v.get("TOF").get<double>());
    
    MAC1 = v.get("MAC1").get<string>();
    MAC2 = v.get("MAC2").get<string>();
    
    printLog("MAC1 en string: %s", MAC1);
    printLog("MAC2 en string: %s", MAC2);
    data->ToF  = v.get("TOF").get<double>();
    
    printLog("Se leen los datos del JSON\r\n");
    
    printLog("ToF = %f", data->ToF);
    printLog(" MAC1 = %s", data->MAC1);
    printLog(" MAC2 = %s\r\n", data->MAC2);
    
    BLEMail.put(data);
    
    printLog("Se introduce dato en BLEqueue\r\n");
}

void writeCharCallback(const GattWriteCallbackParams *params) {
    if(params->handle == tofService->getValueHandle()) {
        char toChar [TOFService::TOF_CHAR_ARRAY_SIZE];
        printLog("Data received: length = %d, data = 0x", params->len);
        for(int x=0; x < params->len; x++) {
            toChar[x] = (char) params->data[x];
            printLog("%x", params->data[x]);
        }
        //toChar[params->len] = '\0';
        printLog("\n\r");
        
        printLog("Cadena: %s\n\r", toChar);
        string str(toChar);
        eventQueue.call(parseRawJSONToQueue, str);
    }
}

/**
 * Esta función se llama si ha habido algún error en el proceso de inicialización del BLE
 */
void onBleInitError(BLE &ble, ble_error_t error) {
    printLog("Ha ocurrido un error al inicializar la configuracion del BLE\n");
}

/**
 * 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) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);
    ble.gattServer().onDataWritten(writeCharCallback);
    
    tofService = new TOFService(ble);
    
    /* Setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); // BLE only, no classic BT
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); // advertising type
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); // add name
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); // UUID's broadcast in advertising packet
    ble.gap().setAdvertisingInterval(100); // 100ms.

    /* Start advertising */
    ble.gap().startAdvertising();
}

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

void BLEServiceManagment() {
    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing); 
    ble.init(bleInitComplete);
    
    eventQueue.dispatch_forever();
}

int main() { 
    srand(time(NULL));
    threadLED.start(callback(blinkLED3));
    threadBLE.start(callback(BLEServiceManagment));
    threadSerial.start(callback(sendJsonOverSerial));
    threadCalculateEvent.start(callback(calculateEvent));
            
    threadLED.join();
    threadBLE.join();
    threadSerial.join();
    threadCalculateEvent.join();
}