main.cpp

00001 /* 
00002  * read and print acc, gyro,temperature date from MPU9250
00003  * and transform accelerate data to one dimension.
00004  * in terminal:
00005  *  ls /dev/tty.*
00006  *  screen /dev/tty.usbmodem14102 9600
00007  * to see the result
00008  *
00009  * mbed Microcontroller Library
00010  * Eigen Library
00011  */
00012 
00013 #include <events/mbed_events.h>
00014 #include <mbed.h>
00015 #include "ble/BLE.h"
00016 #include "ble/Gap.h"
00017 #include "pretty_printer.h"
00018 
00019 #include "mbed.h"
00020 #include "platform/mbed_thread.h"
00021 #include "stats_report.h"
00022 #include "MPU9250.h"
00023 #include <iostream>
00024 #include <vector>  
00025 #include <complex>
00026 #include "BiQuad.h"
00027 #include "pca.h"
00028 #include "peak.h"
00029 
00030 using namespace std;
00031 using namespace Eigen;
00032 
00033 AnalogIn ain(A0);
00034 DigitalOut led1(LED1);
00035 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
00036 //the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719
00037 BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
00038 BiQuadChain bqc;
00039 
00040 
00041 #define SLEEP_TIME                  80 // (msec)
00042 
00043 
00044 const static char DEVICE_NAME[] = "STEP COUNTER";
00045 
00046 const static uint16_t STEP_COUNTER_SERVICE_UUID = 0xA000;
00047 const static uint16_t STEP_COUNTER_CHARACTERISTIC_UUID = 0xA001;
00048 
00049 int step_count = 0;
00050 int id = 0;
00051 ReadWriteGattCharacteristic<int> step_count_state(STEP_COUNTER_CHARACTERISTIC_UUID, &step_count, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00052 
00053 static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE);
00054 
00055 Thread t;
00056 
00057 class StepCounter : ble::Gap::EventHandler {
00058 public:
00059     StepCounter(BLE &ble, events::EventQueue &event_queue) :
00060         _ble(ble),
00061         _event_queue(event_queue),
00062         _step_counter_uuid(STEP_COUNTER_SERVICE_UUID),
00063         _adv_data_builder(_adv_buffer) { }
00064 
00065     void start() {
00066         _ble.gap().setEventHandler(this);
00067 
00068         _ble.init(this, &StepCounter::on_init_complete);
00069         
00070         _event_queue.call_every(500, this, &StepCounter::blink);
00071         _event_queue.call_every(1000, this, &StepCounter::update_step_count);
00072         
00073         t.start(callback(&_event_queue, &EventQueue::dispatch_forever));
00074     }
00075     void update_step_count() {
00076         if (_ble.gap().getState().connected) {
00077             _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
00078         }
00079     }
00080 
00081 private:
00082     /** Callback triggered when the ble initialization process has finished */
00083     void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
00084         if (params->error != BLE_ERROR_NONE) {
00085             print_error(params->error, "Ble initialization failed.");
00086             return;
00087         }
00088         
00089         _ble.gattServer().onDataWritten(this, &StepCounter::on_data_written);
00090 
00091         print_mac_address();
00092 
00093         start_advertising();
00094     }
00095 
00096     void start_advertising() {
00097         /* Create advertising parameters and payload */
00098 
00099         ble::AdvertisingParameters adv_parameters(
00100             ble::advertising_type_t::CONNECTABLE_UNDIRECTED,
00101             ble::adv_interval_t(ble::millisecond_t(1000))
00102         );
00103 
00104         _adv_data_builder.setFlags();
00105         _adv_data_builder.setLocalServiceList(mbed::make_Span(&_step_counter_uuid, 1));
00106         _adv_data_builder.setName(DEVICE_NAME);
00107 
00108         /* Setup advertising */
00109 
00110         ble_error_t error = _ble.gap().setAdvertisingParameters(
00111             ble::LEGACY_ADVERTISING_HANDLE,
00112             adv_parameters
00113         );
00114 
00115         if (error) {
00116             print_error(error, "_ble.gap().setAdvertisingParameters() failed");
00117             return;
00118         }
00119 
00120         error = _ble.gap().setAdvertisingPayload(
00121             ble::LEGACY_ADVERTISING_HANDLE,
00122             _adv_data_builder.getAdvertisingData()
00123         );
00124 
00125         if (error) {
00126             print_error(error, "_ble.gap().setAdvertisingPayload() failed");
00127             return;
00128         }
00129 
00130         /* Start advertising */
00131 
00132         error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
00133 
00134         if (error) {
00135             print_error(error, "_ble.gap().startAdvertising() failed");
00136             return;
00137         }
00138     }
00139     
00140     void on_data_written(const GattWriteCallbackParams *params) {
00141         if ((params->handle == step_count_state.getValueHandle()) && (params->len == 1)) {
00142             step_count = *(params->data);
00143         }
00144         step_count = 0;
00145     }
00146 
00147     void blink(void) {
00148         led1 = !led1;
00149     }
00150 
00151 private:
00152     /* Event handler */
00153 
00154     void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) {
00155         _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
00156     }
00157 
00158 private:
00159     BLE &_ble;
00160     events::EventQueue &_event_queue;
00161     
00162     UUID _step_counter_uuid;
00163 
00164     uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
00165     ble::AdvertisingDataBuilder _adv_data_builder;
00166 };
00167 
00168 /** Schedule processing of events from the BLE middleware in the event queue. */
00169 void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
00170     event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
00171 }
00172 
00173 
00174 // main() runs in its own thread in the OS
00175 int main()
00176 {   
00177     BLE &ble = BLE::Instance();
00178     
00179     GattCharacteristic *charTable[] = {&step_count_state};
00180     GattService step_count_service(STEP_COUNTER_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
00181     ble.addService(step_count_service);
00182     
00183     ble.onEventsToProcess(schedule_ble_events);
00184 
00185     StepCounter demo(ble, event_queue);
00186     demo.start();
00187 
00188     //new mpu(data,clk,address),in constructor addr7bit<<1
00189     mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
00190     //scale of acc and gyro
00191     mpu->initMPU9250(0x00,0x00);
00192 
00193     float AccRead[3];
00194     float GyroRead[3];
00195     float TempRead[1];
00196     float res_smooth;
00197     float threshold=0.1;
00198     int number=0;
00199     int numpeak = 0;
00200     
00201     MatrixXd acc_raw(3,0);
00202     Vector3d acc_new;
00203     Vector3d acc_previous;
00204     MatrixXd C;
00205     MatrixXd vec, val;
00206     int dim = 1;    //dimension of PCA
00207     //use the class defined in pca.h and peak.h
00208     PCA pca;
00209     PEAK peak;
00210     
00211     bqc.add(&mybq);
00212     
00213     acc_new << 0,0,0;
00214     while (true) {
00215         thread_sleep_for(SLEEP_TIME);
00216         //read and convert date
00217         mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
00218         AccRead[0]= AccRead[0]/1000;
00219         AccRead[1]= AccRead[1]/1000;
00220         AccRead[2]= AccRead[2]/1000;
00221         
00222         //append new data to matrix acc_raw
00223         //adding the columns
00224         acc_previous = acc_new;
00225         acc_new << AccRead[0],AccRead[1],AccRead[2];
00226         
00227         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00228         acc_raw.col(acc_raw.cols()-1) = acc_new;
00229         
00230         if(number % 10 ==2)
00231         {
00232             if(number > 2)
00233             {
00234                 //run PCA
00235                 MatrixXd X1=pca.featurnormail(acc_raw);
00236                 pca.ComComputeCov(X1, C);
00237                 pca.ComputEig(C, vec, val);
00238                 //select dim num of eigenvector from right to left. right is important
00239                 //compute the result array
00240                 MatrixXd res = -vec.rightCols(dim).transpose()*X1;
00241                 
00242                 //show the result after PCA
00243                 vector<float> res_list={};
00244                 
00245                 for(int i = 0; i < res.cols(); i++)
00246                 {
00247                     res_list.push_back(res(i));
00248                 }
00249                 int len = res_list.size();
00250                 numpeak = peak.findPeaks(res_list,len,threshold);
00251                 step_count += numpeak;
00252                 
00253                 //clear the matrix to contain new data
00254                 acc_raw.conservativeResize(3, 0);
00255                 
00256                 //overlap windows
00257                 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00258                 acc_raw.col(acc_raw.cols()-1) = acc_previous;
00259                 
00260                 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00261                 acc_raw.col(acc_raw.cols()-1) = acc_new;
00262             }
00263         }
00264         number = number +1;
00265     }
00266 }