main.cpp

00001 #include <events/mbed_events.h>
00002 #include <math.h> 
00003 
00004 #include <ctime>
00005 #include <mbed.h>
00006 #include "MPU9250.h"
00007 #include "ble/BLE.h"
00008 #include "ble/Gap.h"
00009 #include "ble/services/HeartRateService.h"
00010 
00011 DigitalOut Led0(p31);
00012 DigitalOut Led1(p30);
00013 DigitalOut Led2(p29);
00014 DigitalOut Led3(p28);
00015 DigitalOut Led4(p4);
00016 DigitalOut Led5(p3);
00017 DigitalOut Led6(p11);
00018 DigitalOut Led7(p12);
00019 DigitalOut Led8(p13);
00020 DigitalOut Led9(p14);
00021 DigitalOut Led10(p15);
00022 DigitalOut Led11(p16);
00023 DigitalOut Led12(p17);
00024 DigitalOut Led13(p18);
00025 DigitalOut Led14(p19);
00026 DigitalOut Led15(p20);
00027 DigitalOut Led16(p22);
00028 DigitalOut Led17(p23);
00029 DigitalOut Led18(p24);
00030 DigitalOut Led19(p25);
00031 
00032 DigitalIn pb(p8);
00033 
00034 int USV_count = 1;
00035 int old_pb = 1;
00036 int new_pb;
00037     
00038 const static char     DEVICE_NAME[] = "ProVaida";
00039 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
00040 short hrmCounter = 0;
00041 static HeartRateService* hrService;
00042 
00043 bool BLE_conn = false;
00044 
00045 int16_t destination[3];
00046 double step_threshold = 14.45;
00047 
00048 double oldAcceleration = 0.0;
00049 int callback_cycles = 1;
00050 int step;
00051 int step_buffer;
00052 int totalsteps = 0;
00053 
00054 int run_threshold = 5;
00055 int run_count = 0;
00056 
00057 MPU9250 mpu = MPU9250(P0_26, P0_27);
00058 
00059 static EventQueue eventQueue(16 * EVENTS_EVENT_SIZE);
00060 
00061 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00062 {
00063     BLE::Instance().gap().startAdvertising();
00064 }
00065 
00066 void updateSensorValue() {    
00067     wait(.001);
00068     new_pb = pb;
00069     if ((new_pb == 0) && (old_pb == 1)) {
00070         USV_count++;
00071         printf("count: %i\n", USV_count);
00072     }
00073     if (USV_count % 2) {        
00074         step = 0;
00075         callback_cycles++;
00076     
00077         mpu.readAccelData(destination);
00078     
00079         double acc_x = destination[0] / 1000.0;
00080         double acc_y = destination[1] / 1000.0;
00081         double acc_z = destination[2] / 1000.0;
00082     
00083         double sqr_acc_x = acc_x*acc_x;
00084         double sqr_acc_y = acc_y*acc_y;
00085         double sqr_acc_z = acc_z*acc_z;
00086     
00087         double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
00088         double accel = sqrt(sum_acc);
00089     
00090         if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
00091             step_buffer++;
00092             //reached running speed
00093             if (callback_cycles <= run_threshold) {
00094                 if (run_count >= 2) {
00095                     step = 2;
00096                 }
00097                 else {
00098                     step = 1;
00099                     run_count++;
00100                 }
00101             }
00102             //at walking speed
00103             else {
00104                 step = 1;
00105                 run_count = 0;    
00106             }
00107             callback_cycles = 0;
00108         }
00109         if (step != 0)
00110         {
00111             totalsteps++;                
00112             Led0 = ((totalsteps >> 0) % 2);
00113             Led1 = ((totalsteps >> 1) % 2);
00114             Led2 = ((totalsteps >> 2) % 2);
00115             Led3 = ((totalsteps >> 3) % 2);
00116             Led4 = ((totalsteps >> 4) % 2);
00117             Led5 = ((totalsteps >> 5) % 2);
00118             Led6 = ((totalsteps >> 6) % 2);
00119             Led7 = ((totalsteps >> 7) % 2);
00120             Led8 = ((totalsteps >> 8) % 2);
00121             Led9 = ((totalsteps >> 9) % 2);
00122             Led10 = ((totalsteps >> 10) % 2);
00123             Led11 = ((totalsteps >> 11) % 2);
00124             Led12 = ((totalsteps >> 12) % 2);
00125             Led13 = ((totalsteps >> 12) % 2);
00126             Led14 = ((totalsteps >> 14) % 2);
00127             Led15 = ((totalsteps >> 15) % 2);
00128             Led16 = ((totalsteps >> 16) % 2);
00129             Led17 = ((totalsteps >> 17) % 2);
00130             Led18 = ((totalsteps >> 18) % 2);
00131             Led19 = (step == 2);
00132             
00133             if ((totalsteps >> 19) % 2) {
00134                 totalsteps = 0;
00135             }   
00136         }
00137         oldAcceleration = accel;
00138 
00139     } else if (totalsteps != 0) {
00140        totalsteps = 0;
00141        step_buffer = 0;
00142     }         
00143     old_pb = new_pb;    
00144     
00145     if (BLE_conn) {
00146         step_buffer--;
00147         if (step_buffer > 0) {
00148             hrService->updateHeartRate(-1);
00149             while (step_buffer > 127) {
00150                 hrService->updateHeartRate(127);
00151                 step_buffer -= 127;
00152             }
00153             hrService->updateHeartRate(step_buffer);
00154             step_buffer = 0;
00155             hrService->updateHeartRate(-1);
00156         }
00157         hrmCounter = (short) step;    
00158         hrService->updateHeartRate(hrmCounter);        
00159         BLE_conn = false;
00160     }
00161 }
00162 
00163 void blinkCallback(void)
00164 {
00165     BLE &ble = BLE::Instance();
00166     if (ble.gap().getState().connected) {
00167         BLE_conn = true;        
00168     }
00169     eventQueue.call(updateSensorValue);
00170 
00171 }
00172 
00173 void onBleInitError(BLE &ble, ble_error_t error) {}
00174 
00175 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
00176 {
00177     BLE&        ble   = params->ble;
00178     ble_error_t error = params->error;
00179 
00180     if (error != BLE_ERROR_NONE) {        
00181         onBleInitError(ble, error);
00182         return;
00183     }
00184 
00185     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
00186         return;
00187     }
00188 
00189     ble.gap().onDisconnection(disconnectionCallback);
00190 
00191     hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
00192 
00193     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
00194     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
00195     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
00196     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00197     ble.gap().setAdvertisingInterval(1000); 
00198     ble.gap().startAdvertising();
00199 }
00200 
00201 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
00202     BLE &ble = BLE::Instance();
00203     eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
00204 }
00205 
00206 int main()
00207 {   
00208     
00209     pb.mode(PullUp);
00210     mpu.initMPU9250();
00211     
00212     eventQueue.call_every(80, blinkCallback);
00213 
00214     BLE &ble = BLE::Instance();
00215     ble.onEventsToProcess(scheduleBleEventsProcessing);
00216     ble.init(bleInitComplete);
00217 
00218     eventQueue.dispatch_forever();
00219 
00220     return 0;
00221 }