File content as of revision 4:83c7e402fff1:

/* 
 * read and print acc, gyro,temperature date from MPU9250
 * and transform accelerate data to one dimension.
 * in terminal:
 *  ls /dev/tty.*
 *  screen /dev/tty.usbmodem14102 9600
 * to see the result
 *
 * mbed Microcontroller Library
 * 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"
#include "MPU9250.h"
//#include <Eigen/Dense.h>
#include <iostream>
#include <vector>  
#include <complex>
#include "BiQuad.h"
#include "pca.h"
#include "peak.h"

using namespace std;
using namespace Eigen;

DigitalOut led1(LED1);
const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
//the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719
BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
BiQuadChain bqc;


#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
    mpu->initMPU9250(0x00,0x00);

    float AccRead[3];
    float GyroRead[3];
    float TempRead[1];
    float res_smooth;
    //vector<float> res_list;
    float threshold=0.1;
    int number=0;
    int numpeak = 0;
    
    static MatrixXd acc_raw(3,0);
    Vector3d acc_new;
    MatrixXd C;
    MatrixXd vec, val;
    int dim = 1;    //dimension of PCA
    //use the class defined in pca.h and peak.h
    PCA pca;
    PEAK peak;
    
    bqc.add(&mybq);
    
    static vector<float> res_list;
    
    while (true) {
        
        //when i add to 600, there accure faugment error
        //printf("the %f loop", number);
        number = number +1;
        //vector<float> res_list;
        //Blink LED and wait 1 seconds
        //led1 = !led1;
        //thread_sleep_for(SLEEP_TIME);
        //read and convert date
        mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
        AccRead[0]= AccRead[0]/1000;
        AccRead[1]= AccRead[1]/1000;
        AccRead[2]= AccRead[2]/1000;
        printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
        //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
        //printf("temp value is %f.\n\r",TempRead[0]);
        
        //append new data to matrix acc_raw
        //adding the columns
        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;
        printf("before if\n\r");
        printf("number: %d\n\r", number);
        if(number % 10 ==2)
        {
            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);
            }
        }
        number = number +1;
    }
}