File content as of revision 5:ffe1a4c4881e:

#include "mbed.h"
#include "rtos.h"
#include <events/mbed_events.h>

#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/services/BatteryService.h"
#include "DOORService.h"
#include "BNO055.h"
#include "AS7000.h"
#include "EthernetInterface.h"
#include "WIFIDevice.h"

BNO055 imu(p0,p30);
AS7000 hrm(p0,p30);
Serial pc(USBTX, USBRX);

Semaphore two_slots(2);
//static Thread t1;
static Thread t2;
static Ticker ticker;

const static char     DEVICE_NAME[] = "DOORCTL";
static uint8_t ADV_manuf[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
static char out_buffer_tt[] = "hrm:120 yaw:250.2 pitch:359.1 roll:180.2\n";
static uint8_t BLE_RX_CMD = 0xFF;
static uint8_t doorStatusPayload[2] = {0xFF,};
union IP {
    unsigned int ip;
    struct {
      unsigned char d;
      unsigned char c;
      unsigned char b;
      unsigned char a;
    } ip2;
};

char  ips[20];
IP ip;
unsigned short a, b, c, d;
BLEDevice  ble;
EthernetInterface eth;
WIFIDevice wifi;
TCPSocketConnection sock_tcp;
//UDPSocket sock_udp;
//Endpoint echo_server;

static DOORService *doorServicePtr;
static EventQueue eventQueue(
    /* event count */ 16 * /* event size */ 32
);
static char* ECHO_SERVER_ADDRESS = "192.168.2.110";
static int ECHO_SERVER_PORT = 1030;
static bool triggerTempValueUpdate = false;

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

void onDataWrittenCallback(const GattWriteCallbackParams *params) {
    
    
    if ((params->handle == doorServicePtr->getValueHandle()) && (params->len == 1)) {
        {
            BLE_RX_CMD = *(params->data);            
        }
    }
}

void ble_thread() {
    while (true) {
         switch (BLE_RX_CMD) {               
            case 0x00:            
                if ( ADV_manuf[5]== 0xA1) {      
                    eth.disconnect();
                    sock_tcp.close();                                 
                    wifi.sleep();  
                }       
                        
                BLE_RX_CMD = 0xFF;
                doorStatusPayload[0]=0xF0;
                ADV_manuf[5] = 0xF0;   
                               
                eth.init();  
                wifi.setNetwork(M2M_WIFI_SEC_WPA_PSK, "20-4F-5_Guest", "11223344"); 
                eth.connect();    
                /*
                if(wifi.is_AP_connected())
                    printf("Connect Success! \n");
                else
                    printf("Connect Fail! \n");              
                
                printf("MAC: %s\n", eth.getMACAddress());            
                printf("IP: %s\n", eth.getIPAddress());
                printf("Gateway: %s\n", eth.getGateway());
                printf("NetworkMask: %s\n", eth.getNetworkMask());  
                */
                while (sock_tcp.connect(ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT) < 0) {
                    printf("Unable to connect to (%s) on port (%d)\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
                    wait(1);
                }
                //printf("Connected to Server at %s\n",ECHO_SERVER_ADDRESS);
                
                snprintf(ips, sizeof(ips), "%s",eth.getIPAddress());    
                unsigned short a, b, c, d;
                sscanf(ips, "%hu.%hu.%hu.%hu", &a, &b, &c, &d);    
                sprintf(ips, "%x.%x.%x.%x", a, b, c, d);
                ADV_manuf[0] = a;
                ADV_manuf[1] = b;
                ADV_manuf[2] = c;
                ADV_manuf[3] = d;                                 
            break;
                       
            case 0x02:
                if ( ADV_manuf[5]== 0xA1) {  
                eth.disconnect();
                sock_tcp.close();                                 
                wifi.sleep();   
                } 
                BLE_RX_CMD = 0xFF;
                 
                ADV_manuf[0] = 0x00;
                ADV_manuf[1] = 0x00;
                ADV_manuf[2] = 0x00;
                ADV_manuf[3] = 0x00;
                
                ADV_manuf[5] = 0xF2;
                doorStatusPayload[0]=0xF2;
                              
            break;

            case 0xA1:
                //BLE_RX_CMD = 0xFF;   
                sprintf(out_buffer_tt,"hrm:%03d yaw:%6.2f pitch:%6.2f roll:%6.2f\n", hrm.hrm.hreat_rate, imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
                sock_tcp.send_all(out_buffer_tt, sizeof(out_buffer_tt) - 1);                                     
                ADV_manuf[5] = 0xA1;  
                doorStatusPayload[0]=0xA1;  
            break;         
        }
            
       if (ble.getGapState().connected) {
            ble.updateCharacteristicValue((doorServicePtr->getValueHandle()),doorStatusPayload, 1);           
        }
    }
}



/**
 * 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);
    ble.gattServer().onDataWritten(onDataWrittenCallback);
    
    /* Setup primary services */  
    uint8_t initialValueForDOORCharacteristic = 0xFF;
    doorServicePtr = new DOORService(ble, initialValueForDOORCharacteristic);

    /* Setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, ADV_manuf, sizeof(ADV_manuf));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(200); /* 1000ms */
    ble.gap().startAdvertising();
}

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}
    
void network_thread(void const *name) {
    while (true) {
       two_slots.wait();
        //printf("%s\n\r", (const char*)name);
        ble_thread();  
 /*            
        wait_ms(5);
        imu.get_angles(); //query the i2c device
        //printf("get angles\n");
        wait_ms(5);
        hrm.hr_only();
        //printf("get hrm\n");
*/
        wait_ms(5);
      two_slots.release();
    }
}

void periodicCallback(void){
     triggerTempValueUpdate = true;
}

int main (void) {
    pc.baud(38400);
    hrm.enable(); 
    imu.reset();      
    imu.setmode(OPERATION_MODE_NDOF);
    
    
    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);

   // t1.start(callback(sensor_thread, (void *)"Sensor Thread"));    
    t2.start(callback(network_thread, (void *)"BLE-WIFI CTL Thread"));


    ticker.attach(periodicCallback, 1); // blink LED every 1 second


    while (true) {
            if (triggerTempValueUpdate) {
                /* Do blocking calls or whatever hardware-specific action is
                 * necessary to poll the sensor. */
                BLE::Instance(BLE::DEFAULT_INSTANCE).gap().updateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, ADV_manuf, sizeof(ADV_manuf));
                printf("*HR=%03d#", hrm.hrm.hreat_rate);
                triggerTempValueUpdate = false;
            }
        hrm.hr_only();
        wait_ms(15);
        imu.get_angles(); //query the i2c device
        wait_ms(15);
        ble.waitForEvent();
    }
}