File content as of revision 8:a0e1222bd768:

#include "mbed.h"
#include "defineGPIOs.h"
#include "BLE.h"
//#include "HealthThermometerService.h"
#include "ble/services/HealthThermometerService.h"  // added
#include "PresenceDetectionService.h"
#include "HumidityMeasureService.h"
#include "ble_gatt.h"
#include "Si7020.h"
#include "UARTService.h"
#include <string.h>
#include "DiscoveredCharacteristic.h"
#include "DiscoveredService.h"


// OFFSET TEMPERATURE CONFIGURATIONS:
#define STANDARD -1  // Squared radar sensor module: -1ºC
#define ZERO 0  // no offset
#define OFFSET STANDARD

#define NEED_SERIAL_CONSOLE_OUTPUT 1
#if NEED_SERIAL_CONSOLE_OUTPUT
Serial pc(PIN_TX, PIN_RX);
#define SERIAL_DEBUG(...) { printf(__VA_ARGS__); }  //Defaults to stdio without having to wirte pcUart explicitly
#else
#define SERIAL_DEBUG(...) /* nothing */
#endif

#define NEED_BLE_OUTPUT 0 // Set this if you need debug messages on the console;
#if NEED_BLE_OUTPUT
#define DEBUG(STR) { if (uart) uart->write(STR, strlen(STR)); }
#else
#define DEBUG(...) /* nothing */
#endif /* #if NEED_CONSOLE_OUTPUT */

#define CHARACTERISTIC_PRES_UUID 0xA001
#define CHARACTERISTIC_TEMP_UUID 0x2A1C
#define CHARACTERISTIC_HUM_UUID 0xA005

#define SERVICE_PRES_UUID 0xA000
#define SERVICE_TEMP_UUID 0x1809
#define SERVICE_HUM_UUID 0xA004

I2C i2c(PIN_SDA, PIN_SCL);
Si7020 tempsensor(&i2c);

InterruptIn     motion_pin(PIN_PRESENCE_RIGHT);
DigitalIn       pinR(PIN_PRESENCE_RIGHT, PullNone);
DigitalOut      ledB(PIN_BLED_PCB, 1);
DigitalOut      ledR(PIN_RLED_PCB, 1);

//bool presenceState = false;
int presenceCounter = 0;
bool installMode = 1;   // to turn on leds and fast update of BLE services
bool battNotify = 1;
static volatile bool  triggerPresencePolling = false;
static volatile bool  triggerSensorPolling  = false;


/* BLE VARS CONFIGURATION */

BLE ble;

bool foundPresenceCharacteristic = false;
bool foundTemperatureCharacteristic = false;
bool foundHumidityCharacteristic = false;

DiscoveredCharacteristic presenceCharacteristic;
DiscoveredCharacteristic temperatureCharacteristic;
DiscoveredCharacteristic humidityCharacteristic;

PresenceDetectionService *presenceService = NULL;
static HealthThermometerService *thermometerServicePtr;
HumidityMeasureService *humidityService = NULL;
UARTService *uart;

int     neighborPresence = 0;
float   neighborTemperature = 0;
int     neighborHumidity = 0;

uint8_t     PEER_DEV_NAME[4] = {0x53,0x4D,0x3F,0x3F};   // = SM??
static const char       DEVICE_NAME[]         = "SM01";
static const uint16_t   uuid16_list[]         = {GattService::UUID_HEALTH_THERMOMETER_SERVICE};
static char             fwversion[31]         = "SON-2017";
static uint8_t          batteryLevel          = 100;
static int8_t           TxPower               = +4;
const static uint8_t    ManufData[] = {0x01,0x02,0x03,0x04,0x05};   // Set up to 26B of advertising data to use for the Manufacturer data.

// Firmware
GattCharacteristic  fwChars(GattCharacteristic::UUID_FIRMWARE_REVISION_STRING_CHAR, (uint8_t *)fwversion, sizeof(fwversion), sizeof(fwversion),GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic *firmwareChars[] = {&fwChars };
GattService         firmwareService(GattService::UUID_DEVICE_INFORMATION_SERVICE, firmwareChars, sizeof(firmwareChars) / sizeof(GattCharacteristic *));

//Battery
GattCharacteristic  batteryPercentage(GattCharacteristic::UUID_BATTERY_LEVEL_CHAR, (uint8_t *)batteryLevel, sizeof(batteryLevel), sizeof(batteryLevel), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
GattCharacteristic *batteryChars[] = {&batteryPercentage};
GattService         batteryService(GattService::UUID_BATTERY_SERVICE, batteryChars, sizeof(batteryChars) / sizeof(GattCharacteristic *));

//Power
GattCharacteristic  TxPowerChar(GattCharacteristic::UUID_TX_POWER_LEVEL_CHAR, (uint8_t*)&TxPower, sizeof(TxPower), sizeof(TxPower), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic *charTable[] = {&TxPowerChar};
GattService         TxPowerService(GattService::UUID_TX_POWER_SERVICE, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));

/* BATTERY MEASUREMENT */
void my_analogin_init(void)
{
    NRF_ADC->ENABLE = ADC_ENABLE_ENABLE_Enabled;
    NRF_ADC->CONFIG = (ADC_CONFIG_RES_10bit << ADC_CONFIG_RES_Pos) |
                      (ADC_CONFIG_INPSEL_SupplyOneThirdPrescaling << ADC_CONFIG_INPSEL_Pos) |
                      (ADC_CONFIG_REFSEL_VBG << ADC_CONFIG_REFSEL_Pos) |
                      (ADC_CONFIG_PSEL_Disabled << ADC_CONFIG_PSEL_Pos) |
                      (ADC_CONFIG_EXTREFSEL_None << ADC_CONFIG_EXTREFSEL_Pos);
}

uint16_t my_analogin_read_u16(void)
{
    NRF_ADC->CONFIG     &= ~ADC_CONFIG_PSEL_Msk;
    NRF_ADC->CONFIG     |= ADC_CONFIG_PSEL_Disabled << ADC_CONFIG_PSEL_Pos;
    NRF_ADC->TASKS_START = 1;
    while (((NRF_ADC->BUSY & ADC_BUSY_BUSY_Msk) >> ADC_BUSY_BUSY_Pos) == ADC_BUSY_BUSY_Busy) {};
    return (uint16_t)NRF_ADC->RESULT; // 10 bit
}

float getBatteryVolt (void)
{
    char Vpower[10];

    float Vadc = (float)my_analogin_read_u16();
    float Vref = 1.2; // Internal Vref
    float Vcc = 3 * 4 * Vref * Vadc / 1024.0;

    sprintf (Vpower, "Vcc=%.2fV, ", Vcc);
    DEBUG(Vpower);
    return Vcc;
}

int getBatteryPercent ()
{
    char batt_mess[10];
    float Vbat_min = 2; //2;
    float Vbat_max = 3;
    int battLevel = (int) ( ((getBatteryVolt()-Vbat_min) / (Vbat_max-Vbat_min)) *100);

    sprintf (batt_mess, "Vbatt=%i, ", battLevel);
    DEBUG(batt_mess);

    // To avoid values bigger than 100%
    if (battLevel > 100) {
        battLevel = 100;
    }

    // To avoid overflow due to a value lower than Vbat_min
    if (battLevel >= 250) {
        battLevel = 0;
    }

    return battLevel;
}

void blinkLed (bool enable, int color)
{
    if (enable) {
        if (color == 1) {
            ledB=0;
            wait(0.2);
            ledB=1;
            wait(0.2);
        }
        if (color == 2) {
            ledR=0;
            wait(0.2);
            ledR=1;
            wait(0.2);
        }
    } else {
        // do nothing if enable=0
    }
}

uint8_t updatePresenceCounter (int counts)
{
    uint8_t uint8_counter;

    if (counts>=255) {
        uint8_counter = 255;

    } else {
        uint8_counter = uint8_t(counts);
    }

    return uint8_counter;
}
float getTemperature(void)
{
    float temp;

    if(tempsensor.getTemperature(&temp) != 0) {
        DEBUG("Error getting temperature");
        temp = -1;
    }

    // Debugging:
    SERIAL_DEBUG("\nTemperature = %.2f", temp);
    char message[50];
    sprintf (message, "T=%.2fC\n", temp);
    DEBUG(message);

    // Adding offset:
    temp = temp + OFFSET;

    return temp;
}

float getHumidity(void)
{
    float hum;
    if(tempsensor.getHumidity(&hum) != 0) {
        SERIAL_DEBUG("Error getting humidity");
        hum = -1;
    }

    // Debugging:
    SERIAL_DEBUG("\nHumidity = %f", hum);
    char message[50];
    sprintf (message, "RH=%d\n", (int)hum);
    DEBUG(message);

    return hum;
}

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

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
    //ble.gap().getAdvertisingPayload();  // superfluo?
    SERIAL_DEBUG("\n*****************************");
    SERIAL_DEBUG("\n>>Start advertisementCallback");

    // Some nice debugging:
    SERIAL_DEBUG("\nLooking for devices in the neighbourhood....");
    SERIAL_DEBUG("\nThe peer's BLE address: ");
    for(uint8_t index=0; index<=5; index++) {
        SERIAL_DEBUG("%02x ", params->peerAddr[5-index]);  // for such a reason the address is reverse-ordered
    }
    SERIAL_DEBUG("\nThe peer's RSSI value: %i", params->rssi);
    SERIAL_DEBUG("\nThe peer's is ScanResponse: %i", params->isScanResponse);
    SERIAL_DEBUG("\nThe peer's Advertisement type: %i", params->type);
    SERIAL_DEBUG("\nThe peer's advertisement Payload: ");
    for(uint8_t index=0; index<=30; index++) {
        SERIAL_DEBUG("%02x ", params->advertisingData[index]);
    }
    SERIAL_DEBUG("\nThe peer's DEVICE NAME: ");
    for(uint8_t index=23; index<=26; index++) {
        SERIAL_DEBUG("%c", params->advertisingData[index]);
    }
    SERIAL_DEBUG("(");
    for(uint8_t index=23; index<=26; index++) {
        SERIAL_DEBUG("%02x:", params->advertisingData[index]);
    }
    SERIAL_DEBUG(") ");

    /* If the central device hasn't found yet the peripheral:*/
    //if (!deviceFound) {

        // If the peers peripheral does not correpond to the desired PEER_DEV_NAME: exit
        if ( (params->advertisingData[23] != PEER_DEV_NAME[0]) || (params->advertisingData[24] != PEER_DEV_NAME[1])
                || (params->advertisingData[25] != PEER_DEV_NAME[2]) || (params->advertisingData[26] != PEER_DEV_NAME[3]) ) {

            SERIAL_DEBUG("\n>PEER DEVICE NOT FOUND\n");
            return;
            
        } else {
            BLE::Instance().gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
            //deviceFound = true;
            SERIAL_DEBUG("\n CONNECTED!\n");
            blinkLed(installMode,1);
            blinkLed(installMode,1);
        }
    //}
    SERIAL_DEBUG("\n> End advertisementCallback\n");
}

void serviceDiscoveryCallback(const DiscoveredService *service)
{
    SERIAL_DEBUG("\n>>> SERVICE DISCOVERY \n");
    if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        SERIAL_DEBUG("  S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
    } else {
        printf("  S UUID-");
        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            SERIAL_DEBUG("%02x", longUUIDBytes[i]);
        }
        SERIAL_DEBUG(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
    }
    SERIAL_DEBUG("<<< End Service discovery Callback\n\n");
}

void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
{
    SERIAL_DEBUG("  >> CHARACTERISTIC DISCOVERY \n");
    SERIAL_DEBUG("  UUID %x\n", characteristicP->getUUID().getShortUUID());
    SERIAL_DEBUG("      C UUID-%02x valueAttr[%02x] props[%02x] DEVICE_NAME[%02x]\n", characteristicP->getUUID(), characteristicP->getValueHandle(),
                 characteristicP->getProperties().broadcast());

    if (characteristicP->getUUID().getShortUUID() == CHARACTERISTIC_PRES_UUID) {
        SERIAL_DEBUG("-->Found presence characteristic! ** \n");
        presenceCharacteristic      = *characteristicP;
        foundPresenceCharacteristic = true;
    }

    if (characteristicP->getUUID().getShortUUID() == CHARACTERISTIC_TEMP_UUID) {
        SERIAL_DEBUG("-->Found temperature characteristic! ** \n");
        temperatureCharacteristic      = *characteristicP;
        foundTemperatureCharacteristic = true;
    }

    if (characteristicP->getUUID().getShortUUID() == CHARACTERISTIC_HUM_UUID) {
        SERIAL_DEBUG("-->Found humidity characteristic! ** \n");
        humidityCharacteristic      = *characteristicP;
        foundHumidityCharacteristic = true;
    }
    SERIAL_DEBUG("  << End Characteristic discovery Callback\r\n");
}

void discoveryTerminationCallback(Gap::Handle_t connectionHandle)
{
    SERIAL_DEBUG("Terminated Discovery for handle %u\r\n", connectionHandle);
    blinkLed(installMode,1);
    blinkLed(installMode,1);
    blinkLed(installMode,1);
}

void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    SERIAL_DEBUG(">> Connection Callback\r\n");
    if (params->role == Gap::CENTRAL) {
        SERIAL_DEBUG("Launching discovery of BLE services and characteristics...\n");
        //connectionHandle = params->handle;
        ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback);
    }

    SERIAL_DEBUG("> End Connection Callback\r\n");
}

void sendRemoteValues(const GattWriteCallbackParams *response)
{
    SERIAL_DEBUG("Presence resetted \n");
}

void getRemoteValues(const GattReadCallbackParams *response)
{
    SERIAL_DEBUG("\nStart getRemote Values\n");
    if (response->handle == presenceCharacteristic.getValueHandle()) {
#if DUMP_READ_DATA
        printf("triggerToggledWrite: handle %u, offset %u, len %u\r\n", response->handle, response->offset, response->len);
        for (unsigned index = 0; index < response->len; index++) {
            printf("%c[%02x]", response->data[index], response->data[index]);
        }
        printf("\r\n");

#endif
        neighborPresence = response->data[0];
        SERIAL_DEBUG("Neighbour presence: %d", neighborPresence);
    } else {
        SERIAL_DEBUG("\nERROR while getting presence value\n");
    }

    if (response->handle == temperatureCharacteristic.getValueHandle()) {
#if DUMP_READ_DATA
        SERIAL_DEBUG("triggerToggledWrite: handle %u, offset %u, len %u\r\n", response->handle, response->offset, response->len);
        for (unsigned index = 0; index < response->len; index++) {
            printf("%c[%02x]", response->data[index], response->data[index]);
        }
        SERIAL_DEBUG("\nERROR while getting temperature value\n");
        printf("\r\n");
#endif
        neighborTemperature = float( (response->data[1]) + (response->data[2] << 8) ) /100;
        SERIAL_DEBUG("Neighbour temperature: %.2f (%x:%x:%x:%x\n)\n", float(neighborTemperature),response->data[0],response->data[1],response->data[2],response->data[3]);
        //SERIAL_DEBUG("Neighbour temperature(HEX): %x:%x:%x:%x\n", response->data[0],response->data[1],response->data[2],response->data[3]);
    }

    if (response->handle == humidityCharacteristic.getValueHandle()) {
#if DUMP_READ_DATA
        printf("triggerToggledWrite: handle %u, offset %u, len %u\r", response->handle, response->offset, response->len);
        for (unsigned index = 0; index < response->len; index++) {
            printf("%c[%02x]", response->data[index], response->data[index]);
        }
        printf("\r\n");
#endif
        neighborHumidity = response->data[0];
        SERIAL_DEBUG("Neighbour humidity: %f", neighborHumidity);
    } else {
        SERIAL_DEBUG("\nERROR while getting temperature value\n");
    }
    SERIAL_DEBUG("> End getRemote Values\r\n");
}

/* Restart Advertising on disconnection*/
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    SERIAL_DEBUG("> Disconnected\r\n");
    BLE::Instance().gap().startAdvertising();
    ble.gap().setScanParams(500, 100);
    BLE::Instance().gap().startScan(advertisementCallback);
}

void onDataWrittenCallback(const GattWriteCallbackParams *params)
{
    int sel = params->data[0];

    if (sel == 1) {
        blinkLed(installMode,1);
        SERIAL_DEBUG("BLE. Resetting presence\n");
        presenceCounter = 0;
        triggerSensorPolling = true;       // force update of all BLE services
    }

    if (sel == 2) {
        blinkLed(1,1);
        blinkLed(1,1);
        SERIAL_DEBUG("BLE. All LEDs ON/OFF\n");
        installMode = !installMode;
    }

    if (sel == 3) {
        blinkLed(1,1);
        blinkLed(1,1);
        blinkLed(1,1);
        SERIAL_DEBUG("BLE. Rebooting sensor\n");
        wait(3);
        NVIC_SystemReset(); // SW Reset
    }
}

/**
 * Callback triggered when the ble initialization process has finished
 */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    SERIAL_DEBUG("BLE. Init \n");

    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().onConnection(connectionCallback);
    ble.gap().onDisconnection(disconnectionCallback);

    // Set Transmission power:
    ble.setTxPower(TxPower);

    //ble.gap().setScanParams(500, 100);
    //ble.gap().startScan(advertisementCallback);

    //BLE server setup
    ble.gattServer().onDataWritten(onDataWrittenCallback);

    //Setup primary services
    presenceService = new PresenceDetectionService(ble, updatePresenceCounter(presenceCounter));
    thermometerServicePtr = new HealthThermometerService(ble, getTemperature(), HealthThermometerService::LOCATION_EAR);
    humidityService = new HumidityMeasureService(ble, getHumidity());
    ble.addService(batteryService);
    ble.addService(firmwareService);
    uart = new UARTService(ble);
    ble.addService(TxPowerService);


    /* setup advertising */

    /* Sacrifice 3B of 31B to Advertising Flags */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);

    /* Sacrifice 2B of 31B to AdvType overhead, rest goes to AdvData array you define */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, ManufData, sizeof(ManufData));
    ble.gap().accumulateAdvertisingPayloadTxPower(TxPower);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    //ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::THERMOMETER_EAR);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_THERMOMETER);
    //ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
    //ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));

    ble.setAdvertisingInterval(1000); /* 1000ms */
    ble.startAdvertising();
    SERIAL_DEBUG("\nBLE Init completed\n");
}

void periodicCallbackPresence(void)
{
    triggerPresencePolling = true;
    //presenceState = true;
}

void periodicCallbackSensor (void)
{
    triggerSensorPolling = true;
}

void updateBLEservices ()
{
    thermometerServicePtr->updateTemperature(getTemperature());
    humidityService->updateHumidity(getHumidity());
    presenceService->updatePresence(updatePresenceCounter (presenceCounter));
    batteryLevel = getBatteryPercent();
    ble.updateCharacteristicValue(batteryPercentage.getValueAttribute().getHandle(), &batteryLevel, sizeof(batteryLevel));
    ble.updateCharacteristicValue(TxPowerChar.getValueAttribute().getHandle(), (uint8_t*)&TxPower, 1);
}

int main(void)
{
    SERIAL_DEBUG("Start \n");
    char pres_message[20];

    blinkLed(1,1);

    motion_pin.fall(&periodicCallbackPresence);

    Ticker ticker_sensor;
    ticker_sensor.attach(periodicCallbackSensor, 300); // 5 min

    //BLE instance setup
    BLE &bleptr = BLE::Instance();
    bleptr.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) {    }
    */

    while (true) {

        /* Update presence data if presence is detected:*/
        if(triggerPresencePolling) {
            triggerPresencePolling = false;
            presenceCounter++;

            /* if in installMode, blinkleds and update BLE services continuosly */
            if (installMode) {
                updateBLEservices();    // Update BLE services
                blinkLed(1,1);      // blinkLed
            }

            // Debugging: 
            SERIAL_DEBUG("Presence counter: %i\n", presenceCounter);
            sprintf (pres_message, "Pres=%i\n", presenceCounter);
            DEBUG(pres_message);
        }

        // if no presence:
        else {
            SERIAL_DEBUG(".");
        }

        //if (triggerSensorPolling && ble.getGapState().connected) {
        /* Update BLE services only every 300sec or if connected: */
        //if (triggerSensorPolling || ble.getGapState().connected) {

        /* Update BLE services only if connected: */
        if (ble.getGapState().connected) {

            // Quanto consuma quando sta leggendo la temperatura?
            //triggerSensorPolling = false;
            updateBLEservices();

            // Red LED blinks if low battery, at every connection
            if (batteryLevel <= 10) {
                blinkLed(battNotify,2);
            }

        } else {
            ble.waitForEvent();
        }
    }
}