Diff: main.cpp

Revision:

4:83c7e402fff1

Parent:

2:d4c480d17944

Child:

5:8c276289611c

--- a/main.cpp	Mon Nov 25 14:34:22 2019 +0000
+++ b/main.cpp	Tue Nov 26 16:23:08 2019 +0000
@@ -10,6 +10,12 @@
  * Eigen Library
  */
 
+#include <events/mbed_events.h>
+#include <mbed.h>
+#include "ble/BLE.h"
+#include "ble/Gap.h"
+#include "pretty_printer.h"
+
 #include "mbed.h"
 #include "platform/mbed_thread.h"
 #include "stats_report.h"
@@ -35,9 +41,153 @@
 #define SLEEP_TIME                  20 // (msec)
 
 
+const static char DEVICE_NAME[] = "STEP COUNTER";
+
+const static uint16_t STEP_COUNTER_SERVICE_UUID = 0xA000;
+const static uint16_t STEP_COUNTER_CHARACTERISTIC_UUID = 0xA001;
+
+int step_count = 0;
+int id = 0;
+ReadWriteGattCharacteristic<int> step_count_state(STEP_COUNTER_CHARACTERISTIC_UUID, &step_count, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+
+static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE);
+
+Thread t;
+
+class StepCounter : ble::Gap::EventHandler {
+public:
+    StepCounter(BLE &ble, events::EventQueue &event_queue) :
+        _ble(ble),
+        _event_queue(event_queue),
+        _step_counter_uuid(STEP_COUNTER_SERVICE_UUID),
+        _adv_data_builder(_adv_buffer) { }
+
+    void start() {
+        _ble.gap().setEventHandler(this);
+
+        _ble.init(this, &StepCounter::on_init_complete);
+        
+        _event_queue.call_every(500, this, &StepCounter::blink);
+        _event_queue.call_every(3000, this, &StepCounter::update_step_count);
+        
+        t.start(callback(&_event_queue, &EventQueue::dispatch_forever));
+    }
+    void update_step_count() {
+        printf("updating step count %d\n\r", step_count);
+        if (_ble.gap().getState().connected) {
+            _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
+        }
+    }
+
+private:
+    /** Callback triggered when the ble initialization process has finished */
+    void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
+        printf("on init complete\n\r");
+        if (params->error != BLE_ERROR_NONE) {
+            print_error(params->error, "Ble initialization failed.");
+            return;
+        }
+        
+        _ble.gattServer().onDataWritten(this, &StepCounter::on_data_written);
+
+        print_mac_address();
+
+        start_advertising();
+    }
+
+    void start_advertising() {
+        printf("start advertising\n\r");
+        /* Create advertising parameters and payload */
+
+        ble::AdvertisingParameters adv_parameters(
+            ble::advertising_type_t::CONNECTABLE_UNDIRECTED,
+            ble::adv_interval_t(ble::millisecond_t(1000))
+        );
+
+        _adv_data_builder.setFlags();
+        _adv_data_builder.setLocalServiceList(mbed::make_Span(&_step_counter_uuid, 1));
+        _adv_data_builder.setName(DEVICE_NAME);
+
+        /* Setup advertising */
+
+        ble_error_t error = _ble.gap().setAdvertisingParameters(
+            ble::LEGACY_ADVERTISING_HANDLE,
+            adv_parameters
+        );
+
+        if (error) {
+            print_error(error, "_ble.gap().setAdvertisingParameters() failed");
+            return;
+        }
+
+        error = _ble.gap().setAdvertisingPayload(
+            ble::LEGACY_ADVERTISING_HANDLE,
+            _adv_data_builder.getAdvertisingData()
+        );
+
+        if (error) {
+            print_error(error, "_ble.gap().setAdvertisingPayload() failed");
+            return;
+        }
+
+        /* Start advertising */
+
+        error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
+
+        if (error) {
+            print_error(error, "_ble.gap().startAdvertising() failed");
+            return;
+        }
+    }
+    
+    void on_data_written(const GattWriteCallbackParams *params) {
+        if ((params->handle == step_count_state.getValueHandle()) && (params->len == 1)) {
+            step_count = *(params->data);
+        }
+        step_count = 0;
+    }
+
+    void blink(void) {
+        led1 = !led1;
+    }
+
+private:
+    /* Event handler */
+
+    void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) {
+        _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
+    }
+
+private:
+    BLE &_ble;
+    events::EventQueue &_event_queue;
+    
+    UUID _step_counter_uuid;
+
+    uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
+    ble::AdvertisingDataBuilder _adv_data_builder;
+};
+
+/** Schedule processing of events from the BLE middleware in the event queue. */
+void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
+    event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
+}
+
+
 // main() runs in its own thread in the OS
 int main()
 {   
+    BLE &ble = BLE::Instance();
+    
+    GattCharacteristic *charTable[] = {&step_count_state};
+    GattService step_count_service(STEP_COUNTER_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
+    ble.addService(step_count_service);
+    
+    ble.onEventsToProcess(schedule_ble_events);
+
+    StepCounter demo(ble, event_queue);
+    demo.start();
+
     //new mpu(data,clk,address),in constructor addr7bit<<1
     mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
     //scale of acc and gyro
@@ -48,7 +198,9 @@
     float TempRead[1];
     float res_smooth;
     //vector<float> res_list;
-    float number=0;
+    float threshold=0.1;
+    int number=0;
+    int numpeak = 0;
     
     static MatrixXd acc_raw(3,0);
     Vector3d acc_new;
@@ -70,8 +222,8 @@
         number = number +1;
         //vector<float> res_list;
         //Blink LED and wait 1 seconds
-        led1 = !led1;
-        thread_sleep_for(SLEEP_TIME);
+        //led1 = !led1;
+        //thread_sleep_for(SLEEP_TIME);
         //read and convert date
         mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
         AccRead[0]= AccRead[0]/1000;
@@ -86,31 +238,46 @@
         acc_new << AccRead[0],AccRead[1],AccRead[2];
         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
         acc_raw.col(acc_raw.cols()-1) = acc_new;
-        
-        //////cout << "acc_raw:" << acc_raw << endl;
-        
-        //run PCA
-        MatrixXd X1=pca.featurnormail(acc_raw);
-        pca.ComComputeCov(X1, C);
-        pca.ComputEig(C, vec, val);
-        //select dim num of eigenvector from right to left. right is important
-        //compute the result array
-        MatrixXd res = vec.rightCols(dim).transpose()*X1;
-        
-        //show the result after PCA
-        //////cout << "result" << res << endl;
-        res_list.clear();
-        for(int i = 0; i < res.cols(); i++)
+        printf("before if\n\r");
+        printf("number: %d\n\r", number);
+        if(number % 10 ==2)
         {
-            res_smooth = bqc.step(res(i));
-            res_list.push_back(res_smooth);
-            //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
-            //std::cout << "\t" << bqc.step(  ) << std::endl;
+            printf("in if\n\r");
+            if(number > 2)
+            {
+                //cout << acc_raw << endl;
+                //run PCA
+                MatrixXd X1=pca.featurnormail(acc_raw);
+                pca.ComComputeCov(X1, C);
+                pca.ComputEig(C, vec, val);
+                //select dim num of eigenvector from right to left. right is important
+                //compute the result array
+                MatrixXd res = vec.rightCols(dim).transpose()*X1;
+                
+                //show the result after PCA
+                //////cout << "result" << res << endl;
+                vector<float> res_list={};
+                
+                //printf("result of PCA size:%d\n\r",res.cols());
+                for(int i = 0; i < res.cols(); i++)
+                {
+                    res_smooth = bqc.step(res(i));
+                    res_list.push_back(res_smooth);
+                    //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
+                    //std::cout << "\t" << bqc.step(  ) << std::endl;
+                }
+                int len = res_list.size();
+                //printf("len of res:%d\n\r", len);
+                numpeak = peak.findPeaks(res_list,len,threshold);
+                printf("NUMBER OF PEAKS: %d\n\r", numpeak);
+                //printf("height in main: %f\n\r", threshold);
+                step_count += numpeak;
+                printf("num of step: %d\n\r", step_count);
+                
+                //clear the matrix to contain new data
+                acc_raw.conservativeResize(3, 0);
+            }
         }
-        int len = res_list.size();
-        printf("len of res:%d\n\r", len);
-        peak.findPeaks(res_list,len,0.1);
-        
-        
+        number = number +1;
     }
 }