main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2006-2014 ARM Limited
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include <events/mbed_events.h>
00018 #include <math.h> 
00019 
00020 #include <ctime>
00021 #include <mbed.h>
00022 #include "MPU9250.h"
00023 #include "ble/BLE.h"
00024 #include "ble/Gap.h"
00025 #include "ble/services/HeartRateService.h"
00026 
00027 DigitalOut Led0(p31);
00028 DigitalOut Led1(p30);
00029 DigitalOut Led2(p29);
00030 DigitalOut Led3(p28);
00031 DigitalOut Led4(p4);
00032 DigitalOut Led5(p3);
00033 DigitalOut Led6(p11);
00034 DigitalOut Led7(p12);
00035 DigitalOut Led8(p13);
00036 DigitalOut Led9(p14);
00037 
00038 //DigitalOut led1(LED1, 1);
00039 
00040 const static char     DEVICE_NAME[] = "ProVaida";
00041 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
00042 
00043 int16_t destination[3];
00044 short hrmCounter = 0;
00045 double step_threshold = 14.45;
00046 
00047 double oldAcceleration = 0.0;
00048 int callback_cycles = 1; // used to be 4
00049 int step;
00050 int run_threshold = 6; // used to be 7
00051 int run_count = 0;
00052 
00053 int totalsteps = 0;
00054 
00055 static HeartRateService* hrService;
00056 MPU9250 mpu = MPU9250(P0_26, P0_27);
00057 
00058 static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
00059 
00060 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00061 {
00062     BLE::Instance().gap().startAdvertising();
00063 }
00064 
00065 void updateSensorValue() {
00066     step = 0;
00067     callback_cycles++;
00068     
00069     mpu.readAccelData(destination);
00070     
00071     double acc_x = destination[0] / 1000.0;
00072     double acc_y = destination[1] / 1000.0;
00073     double acc_z = destination[2] / 1000.0;
00074     
00075     double sqr_acc_x = acc_x*acc_x;
00076     double sqr_acc_y = acc_y*acc_y;
00077     double sqr_acc_z = acc_z*acc_z;
00078     
00079     double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
00080     double accel = sqrt(sum_acc);
00081 
00082     //printf("calback cycles: " );
00083     //printf("%i\n", callback_cycles);  
00084      
00085     if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 2)) {
00086         if (callback_cycles <= run_threshold) {
00087             if (run_count >= 2) {
00088                 step = 2;
00089             }
00090             else {
00091                 step = 1;
00092                 run_count++;
00093             }
00094         }
00095         else {
00096             step = 1;
00097             run_count = 0;    
00098         }
00099         callback_cycles = 0;
00100     }
00101     if (step != 0)
00102     {
00103         printf("STEP: " );
00104         printf("%i\n", step);          
00105 
00106         totalsteps++;        
00107         
00108         printf("totalSTEPs: " );
00109         printf("%i\n", totalsteps);  
00110         
00111         Led0 = ((totalsteps >> 0) % 2);
00112         Led1 = ((totalsteps >> 1) % 2);
00113         Led2 = ((totalsteps >> 2) % 2);
00114         Led3 = ((totalsteps >> 3) % 2);
00115         Led4 = ((totalsteps >> 4) % 2);
00116         Led5 = ((totalsteps >> 5) % 2);
00117         Led6 = ((totalsteps >> 6) % 2);
00118         Led7 = ((totalsteps >> 7) % 2);
00119         Led8 = ((totalsteps >> 8) % 2);
00120         Led9 = ((totalsteps >> 9) % 2);
00121     }
00122     
00123     oldAcceleration = accel;
00124     
00125     hrmCounter = (short) step;
00126     //printf("STEP: " );
00127     //printf("%hu\n", hrmCounter);  
00128     
00129     hrService->updateHeartRate(hrmCounter);
00130 }
00131 
00132 void blinkCallback(void)
00133 {
00134     //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
00135     BLE &ble = BLE::Instance();
00136     if (ble.gap().getState().connected) {
00137         eventQueue.call(updateSensorValue);
00138     }
00139 }
00140 
00141 /**
00142  * This function is called when the ble initialization process has failed
00143  */
00144 void onBleInitError(BLE &ble, ble_error_t error)
00145 {
00146     /* Initialization error handling should go here */
00147 }
00148 
00149 void printMacAddress()
00150 {
00151     /* Print out device MAC address to the console*/
00152     Gap::AddressType_t addr_type;
00153     Gap::Address_t address;
00154     BLE::Instance().gap().getAddress(&addr_type, address);
00155     printf("DEVICE MAC ADDRESS: ");
00156     for (int i = 5; i >= 1; i--){
00157         printf("%02x:", address[i]);
00158     }
00159     printf("%02x\r\n", address[0]);
00160 }
00161 
00162 /**
00163  * Callback triggered when the ble initialization process has finished
00164  */
00165 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
00166 {
00167     BLE&        ble   = params->ble;
00168     ble_error_t error = params->error;
00169 
00170     if (error != BLE_ERROR_NONE) {
00171         /* In case of error, forward the error handling to onBleInitError */
00172         onBleInitError(ble, error);
00173         return;
00174     }
00175 
00176     /* Ensure that it is the default instance of BLE */
00177     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
00178         return;
00179     }
00180 
00181     ble.gap().onDisconnection(disconnectionCallback);
00182 
00183     /* Setup primary service */
00184     hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
00185 
00186     /* Setup advertising */
00187     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
00188     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
00189     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
00190     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00191     ble.gap().setAdvertisingInterval(1000); /* 1000ms */
00192     ble.gap().startAdvertising();
00193 
00194 
00195     printMacAddress();
00196 
00197 }
00198 
00199 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
00200     BLE &ble = BLE::Instance();
00201     eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
00202 }
00203 
00204 int main()
00205 {    
00206     mpu.initMPU9250();
00207     
00208     eventQueue.call_every(80, blinkCallback);
00209 
00210     BLE &ble = BLE::Instance();
00211     ble.onEventsToProcess(scheduleBleEventsProcessing);
00212     ble.init(bleInitComplete);
00213 
00214     eventQueue.dispatch_forever();
00215 
00216     return 0;
00217 }