Integration of code (not working)
Dependencies: mpu9250_i2c biquadFilter PCA peakdetection Eigen
main.cpp
- Committer:
- nikoletakuneva
- Date:
- 2019-11-26
- Revision:
- 4:83c7e402fff1
- Parent:
- 2:d4c480d17944
- Child:
- 5:8c276289611c
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; } }