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 <mbed.h>
00019 #include "ble/BLE.h"
00020 #include "ble/Gap.h"
00021 #include "pretty_printer.h"
00022 
00023 /***
00024 Library used by PCA
00025 
00026 #include "mbed.h"
00027 #include "platform/mbed_thread.h"
00028 #include "stats_report.h"
00029 #include "MPU9250.h"
00030 #include <Eigen/Dense.h>
00031 #include <iostream>
00032 ***/
00033 
00034 /***
00035 Fields used by PCA
00036 
00037 using namespace std;
00038 using namespace Eigen;
00039 
00040 DigitalOut led1(LED1);
00041 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
00042 
00043 #define SLEEP_TIME 5000 // (msec)
00044 ***/
00045 
00046 /***
00047 Methods used by PCA
00048 
00049 ///*
00050 //  * Normalize the Matrix X
00051 //  */
00052 // MatrixXd featurnormail(MatrixXd &X)
00053 //{
00054 //    //I don't know why to use the transpose
00055 //    //compute the mean of every dimension
00056 //    MatrixXd X1 = X.transpose();
00057 //    MatrixXd meanval = X1.colwise().mean();
00058 //    
00059 //    //normalization
00060 //    RowVectorXd meanvecRow = meanval;
00061 //    X1.rowwise() -= meanvecRow;
00062 //    
00063 //    return X1.transpose();
00064 //}
00065 //
00066 // /*
00067 //  * Compute the Covariane Matrix of X, put to C
00068 //  * C = 1/m * X * X.transpose 
00069 //  */
00070 //void ComComputeCov(MatrixXd &X, MatrixXd &C)
00071 //{
00072 //   
00073 //    C = X*X.adjoint();//same as XT*X a
00074 //    //translate to array
00075 //    C = C.array() / X.cols();
00076 //}
00077 // 
00078 // 
00079 ///*
00080 // * Compute the eigenvalue and eigenvector of C
00081 // * val = (first eigenvalue) --smallest --not important
00082 // *              .
00083 // *              .
00084 // *              .
00085 // *       (last eigenvalue)  --largest -- important
00086 // *
00087 // * vec = (first eigenvector, ... , last eigenvector)
00088 // *           not important          important
00089 // */
00090 //void ComputEig(MatrixXd &C, MatrixXd &vec, MatrixXd &val)
00091 //{
00092 //    //SelfAdjointEigenSolver will sort the values automatically
00093 //    SelfAdjointEigenSolver<MatrixXd> eig(C);
00094 //    vec = eig.eigenvectors();
00095 //    val = eig.eigenvalues();
00096 //}
00097 // 
00098 ///* Compute the dimension need to include enough information of raw data.
00099 // * form large index to small index, since the val is sorted from small to large.
00100 // * in some cases, just decide the number of dimension, instead of compute it.
00101 // */
00102 //int ComputDim(MatrixXd &val)
00103 //{
00104 //    int dim;
00105 //    double sum = 0;
00106 //    for (int i = val.rows() - 1; i >= 0;--i)
00107 //    {
00108 //        sum += val(i, 0);
00109 //        dim = i;
00110 //        if (sum / val.sum()>=0.8)//80% of the information
00111 //            break;
00112 //    }
00113 //    return val.rows() - dim;
00114 //}
00115 ***/
00116 
00117 static DigitalOut led1(LED1, 1);
00118 
00119 const static char DEVICE_NAME[] = "STEP COUNTER";
00120 
00121 const static uint16_t STEP_COUNTER_SERVICE_UUID = 0xA000;
00122 const static uint16_t STEP_COUNTER_CHARACTERISTIC_UUID = 0xA001;
00123 
00124 int step_count = 0;
00125 int id = 0;
00126 ReadWriteGattCharacteristic<int> step_count_state(STEP_COUNTER_CHARACTERISTIC_UUID, &step_count, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00127 
00128 static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE);
00129 
00130 class StepCounter : ble::Gap::EventHandler {
00131 public:
00132     StepCounter(BLE &ble, events::EventQueue &event_queue) :
00133         _ble(ble),
00134         _event_queue(event_queue),
00135         _step_counter_uuid(STEP_COUNTER_SERVICE_UUID),
00136         _adv_data_builder(_adv_buffer) { }
00137 
00138     void start() {
00139         _ble.gap().setEventHandler(this);
00140 
00141         _ble.init(this, &StepCounter::on_init_complete);
00142         _event_queue.call_every(500, this, &StepCounter::blink);
00143         
00144         // TODO Replace this to sync with data reading in order to update step
00145         // counts by exact window.
00146         _event_queue.call_every(1000, this, &StepCounter::update_step_count);
00147 
00148         _event_queue.dispatch_forever();
00149     }
00150 
00151 private:
00152     /** Callback triggered when the ble initialization process has finished */
00153     void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
00154         if (params->error != BLE_ERROR_NONE) {
00155             print_error(params->error, "Ble initialization failed.");
00156             return;
00157         }
00158         
00159         _ble.gattServer().onDataWritten(this, &StepCounter::on_data_written);
00160 
00161         print_mac_address();
00162 
00163         start_advertising();
00164     }
00165 
00166     void start_advertising() {
00167         /* Create advertising parameters and payload */
00168 
00169         ble::AdvertisingParameters adv_parameters(
00170             ble::advertising_type_t::CONNECTABLE_UNDIRECTED,
00171             ble::adv_interval_t(ble::millisecond_t(1000))
00172         );
00173 
00174         _adv_data_builder.setFlags();
00175         _adv_data_builder.setLocalServiceList(mbed::make_Span(&_step_counter_uuid, 1));
00176         _adv_data_builder.setName(DEVICE_NAME);
00177 
00178         /* Setup advertising */
00179 
00180         ble_error_t error = _ble.gap().setAdvertisingParameters(
00181             ble::LEGACY_ADVERTISING_HANDLE,
00182             adv_parameters
00183         );
00184 
00185         if (error) {
00186             print_error(error, "_ble.gap().setAdvertisingParameters() failed");
00187             return;
00188         }
00189 
00190         error = _ble.gap().setAdvertisingPayload(
00191             ble::LEGACY_ADVERTISING_HANDLE,
00192             _adv_data_builder.getAdvertisingData()
00193         );
00194 
00195         if (error) {
00196             print_error(error, "_ble.gap().setAdvertisingPayload() failed");
00197             return;
00198         }
00199 
00200         /* Start advertising */
00201 
00202         error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
00203 
00204         if (error) {
00205             print_error(error, "_ble.gap().startAdvertising() failed");
00206             return;
00207         }
00208     }
00209     
00210     void on_data_written(const GattWriteCallbackParams *params) {
00211         if ((params->handle == step_count_state.getValueHandle()) && (params->len == 1)) {
00212             step_count = *(params->data);
00213         }
00214         step_count = 0;
00215     }
00216 
00217     void update_step_count() {
00218         if (_ble.gap().getState().connected) {
00219             // TODO Remove step_count increament which would be implemented at 
00220             // main() after peak detection.
00221             step_count++;
00222 
00223             _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
00224         }
00225     }
00226 
00227     void blink(void) {
00228         led1 = !led1;
00229     }
00230 
00231 private:
00232     /* Event handler */
00233 
00234     void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) {
00235         _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
00236     }
00237 
00238 private:
00239     BLE &_ble;
00240     events::EventQueue &_event_queue;
00241     
00242     UUID _step_counter_uuid;
00243 
00244     uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
00245     ble::AdvertisingDataBuilder _adv_data_builder;
00246 };
00247 
00248 /** Schedule processing of events from the BLE middleware in the event queue. */
00249 void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
00250     event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
00251 }
00252 
00253 int main()
00254 {
00255     BLE &ble = BLE::Instance();
00256     
00257     GattCharacteristic *charTable[] = {&step_count_state};
00258     GattService step_count_service(STEP_COUNTER_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
00259     ble.addService(step_count_service);
00260     
00261     ble.onEventsToProcess(schedule_ble_events);
00262 
00263     StepCounter demo(ble, event_queue);
00264     demo.start();
00265     
00266     // read data
00267     /***
00268     PCA parameters init
00269     
00270         //new mpu(data,clk,address),in constructor addr7bit<<1
00271     mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
00272     //scale of acc and gyro
00273     mpu->initMPU9250(0x00,0x00);
00274 
00275     float AccRead[3];
00276     float GyroRead[3];
00277     float TempRead[1];
00278     
00279     
00280     MatrixXd acc_raw(3,0);
00281     Vector3d acc_new;
00282     MatrixXd C;
00283     MatrixXd vec, val;
00284     int dim = 1;    //dimension of PCA
00285     ***/
00286     /***
00287     Reading data and do PCA
00288     
00289     while (true) {
00290         
00291         //Blink LED and wait 1 seconds
00292         // TODO Question actually: Is this to wait for collecting enough data
00293         // for a window?
00294         led1 = !led1;
00295         thread_sleep_for(SLEEP_TIME);
00296 
00297         //read and convert date
00298         mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
00299         //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
00300         //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
00301         //printf("temp value is %f.\n\r",TempRead[0]);
00302         
00303         //append new data to matrix acc_raw
00304         //adding the columns
00305         acc_new << AccRead[0],AccRead[1],AccRead[2];
00306         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00307         acc_raw.col(acc_raw.cols()-1) = acc_new;
00308         //cout << "acc_raw:" << acc_raw << endl;
00309         
00310         // TODO Check if there are enough data for a single window
00311         // if true -> run PCA and peak detection
00312         // else    -> sleep for another 1 second maybe?
00313         
00314         //run PCA
00315         MatrixXd X1=featurnormail(acc_raw);
00316         ComComputeCov(X1, C);
00317         ComputEig(C, vec, val);
00318         //select dim num of eigenvector from right to left. right is important
00319         //compute the result array
00320         MatrixXd res = vec.rightCols(dim).transpose()*X1;
00321         
00322         //show the result after PCA
00323         //cout << "result" << res << endl;
00324         
00325         // TODO add peak detection algorithm here
00326         
00327         // TODO add step count.  Also, think about lock of the variable - what
00328         // if 
00329     }
00330     ***/
00331     
00332 
00333     return 0;
00334 }