File content as of revision 22:0d59828e0b66:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2014 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <events/mbed_events.h>
#include <math.h> 

#include <ctime>
#include <mbed.h>
#include "MPU9250.h"
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/services/HeartRateService.h"

DigitalOut Led0(p31);
DigitalOut Led1(p30);
DigitalOut Led2(p29);
DigitalOut Led3(p28);
DigitalOut Led4(p4);
DigitalOut Led5(p3);
DigitalOut Led6(p11);
DigitalOut Led7(p12);
DigitalOut Led8(p13);
DigitalOut Led9(p14);

//DigitalOut led1(LED1, 1);

const static char     DEVICE_NAME[] = "ProVaida";
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};

int16_t destination[3];
short hrmCounter = 0;
double step_threshold = 14.45;

double oldAcceleration = 0.0;
int callback_cycles = 1; // used to be 4
int step;
int run_threshold = 6; // used to be 7
int run_count = 0;

int totalsteps = 0;

static HeartRateService* hrService;
MPU9250 mpu = MPU9250(P0_26, P0_27);

static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);

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

void updateSensorValue() {
    step = 0;
    callback_cycles++;
    
    mpu.readAccelData(destination);
    
    double acc_x = destination[0] / 1000.0;
    double acc_y = destination[1] / 1000.0;
    double acc_z = destination[2] / 1000.0;
    
    double sqr_acc_x = acc_x*acc_x;
    double sqr_acc_y = acc_y*acc_y;
    double sqr_acc_z = acc_z*acc_z;
    
    double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
    double accel = sqrt(sum_acc);

    //printf("calback cycles: " );
    //printf("%i\n", callback_cycles);  
     
    if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 2)) {
        if (callback_cycles <= run_threshold) {
            if (run_count >= 2) {
                step = 2;
            }
            else {
                step = 1;
                run_count++;
            }
        }
        else {
            step = 1;
            run_count = 0;    
        }
        callback_cycles = 0;
    }
    if (step != 0)
    {
        printf("STEP: " );
        printf("%i\n", step);          

        totalsteps++;        
        
        printf("totalSTEPs: " );
        printf("%i\n", totalsteps);  
        
        Led0 = ((totalsteps >> 0) % 2);
        Led1 = ((totalsteps >> 1) % 2);
        Led2 = ((totalsteps >> 2) % 2);
        Led3 = ((totalsteps >> 3) % 2);
        Led4 = ((totalsteps >> 4) % 2);
        Led5 = ((totalsteps >> 5) % 2);
        Led6 = ((totalsteps >> 6) % 2);
        Led7 = ((totalsteps >> 7) % 2);
        Led8 = ((totalsteps >> 8) % 2);
        Led9 = ((totalsteps >> 9) % 2);
    }
    
    oldAcceleration = accel;
    
    hrmCounter = (short) step;
    //printf("STEP: " );
    //printf("%hu\n", hrmCounter);  
    
    hrService->updateHeartRate(hrmCounter);
}

void blinkCallback(void)
{
    //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
    BLE &ble = BLE::Instance();
    if (ble.gap().getState().connected) {
        eventQueue.call(updateSensorValue);
    }
}

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

void printMacAddress()
{
    /* Print out device MAC address to the console*/
    Gap::AddressType_t addr_type;
    Gap::Address_t address;
    BLE::Instance().gap().getAddress(&addr_type, address);
    printf("DEVICE MAC ADDRESS: ");
    for (int i = 5; i >= 1; i--){
        printf("%02x:", address[i]);
    }
    printf("%02x\r\n", address[0]);
}

/**
 * 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 */
    hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);

    /* 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();


    printMacAddress();

}

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

int main()
{    
    mpu.initMPU9250();
    
    eventQueue.call_every(80, blinkCallback);

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

    eventQueue.dispatch_forever();

    return 0;
}