Integration of code (not working)
Dependencies: mpu9250_i2c biquadFilter PCA peakdetection Eigen
main.cpp@4:83c7e402fff1, 2019-11-26 (annotated)
- Committer:
- nikoletakuneva
- Date:
- Tue Nov 26 16:23:08 2019 +0000
- Revision:
- 4:83c7e402fff1
- Parent:
- 2:d4c480d17944
- Child:
- 5:8c276289611c
Integration of code (not working)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
castlefei | 0:44701eab0261 | 1 | /* |
castlefei | 1:92f42e198925 | 2 | * read and print acc, gyro,temperature date from MPU9250 |
castlefei | 1:92f42e198925 | 3 | * and transform accelerate data to one dimension. |
castlefei | 0:44701eab0261 | 4 | * in terminal: |
castlefei | 0:44701eab0261 | 5 | * ls /dev/tty.* |
castlefei | 0:44701eab0261 | 6 | * screen /dev/tty.usbmodem14102 9600 |
castlefei | 1:92f42e198925 | 7 | * to see the result |
castlefei | 0:44701eab0261 | 8 | * |
castlefei | 0:44701eab0261 | 9 | * mbed Microcontroller Library |
castlefei | 1:92f42e198925 | 10 | * Eigen Library |
castlefei | 0:44701eab0261 | 11 | */ |
castlefei | 0:44701eab0261 | 12 | |
nikoletakuneva | 4:83c7e402fff1 | 13 | #include <events/mbed_events.h> |
nikoletakuneva | 4:83c7e402fff1 | 14 | #include <mbed.h> |
nikoletakuneva | 4:83c7e402fff1 | 15 | #include "ble/BLE.h" |
nikoletakuneva | 4:83c7e402fff1 | 16 | #include "ble/Gap.h" |
nikoletakuneva | 4:83c7e402fff1 | 17 | #include "pretty_printer.h" |
nikoletakuneva | 4:83c7e402fff1 | 18 | |
castlefei | 0:44701eab0261 | 19 | #include "mbed.h" |
castlefei | 0:44701eab0261 | 20 | #include "platform/mbed_thread.h" |
castlefei | 0:44701eab0261 | 21 | #include "stats_report.h" |
castlefei | 0:44701eab0261 | 22 | #include "MPU9250.h" |
castlefei | 2:d4c480d17944 | 23 | //#include <Eigen/Dense.h> |
castlefei | 1:92f42e198925 | 24 | #include <iostream> |
castlefei | 2:d4c480d17944 | 25 | #include <vector> |
castlefei | 2:d4c480d17944 | 26 | #include <complex> |
castlefei | 2:d4c480d17944 | 27 | #include "BiQuad.h" |
castlefei | 2:d4c480d17944 | 28 | #include "pca.h" |
castlefei | 2:d4c480d17944 | 29 | #include "peak.h" |
castlefei | 1:92f42e198925 | 30 | |
castlefei | 1:92f42e198925 | 31 | using namespace std; |
castlefei | 1:92f42e198925 | 32 | using namespace Eigen; |
castlefei | 0:44701eab0261 | 33 | |
castlefei | 0:44701eab0261 | 34 | DigitalOut led1(LED1); |
castlefei | 0:44701eab0261 | 35 | const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0 |
castlefei | 2:d4c480d17944 | 36 | //the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719 |
castlefei | 2:d4c480d17944 | 37 | BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457); |
castlefei | 2:d4c480d17944 | 38 | BiQuadChain bqc; |
castlefei | 1:92f42e198925 | 39 | |
castlefei | 1:92f42e198925 | 40 | |
castlefei | 2:d4c480d17944 | 41 | #define SLEEP_TIME 20 // (msec) |
castlefei | 1:92f42e198925 | 42 | |
castlefei | 0:44701eab0261 | 43 | |
nikoletakuneva | 4:83c7e402fff1 | 44 | const static char DEVICE_NAME[] = "STEP COUNTER"; |
nikoletakuneva | 4:83c7e402fff1 | 45 | |
nikoletakuneva | 4:83c7e402fff1 | 46 | const static uint16_t STEP_COUNTER_SERVICE_UUID = 0xA000; |
nikoletakuneva | 4:83c7e402fff1 | 47 | const static uint16_t STEP_COUNTER_CHARACTERISTIC_UUID = 0xA001; |
nikoletakuneva | 4:83c7e402fff1 | 48 | |
nikoletakuneva | 4:83c7e402fff1 | 49 | int step_count = 0; |
nikoletakuneva | 4:83c7e402fff1 | 50 | int id = 0; |
nikoletakuneva | 4:83c7e402fff1 | 51 | ReadWriteGattCharacteristic<int> step_count_state(STEP_COUNTER_CHARACTERISTIC_UUID, &step_count, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
nikoletakuneva | 4:83c7e402fff1 | 52 | |
nikoletakuneva | 4:83c7e402fff1 | 53 | static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE); |
nikoletakuneva | 4:83c7e402fff1 | 54 | |
nikoletakuneva | 4:83c7e402fff1 | 55 | Thread t; |
nikoletakuneva | 4:83c7e402fff1 | 56 | |
nikoletakuneva | 4:83c7e402fff1 | 57 | class StepCounter : ble::Gap::EventHandler { |
nikoletakuneva | 4:83c7e402fff1 | 58 | public: |
nikoletakuneva | 4:83c7e402fff1 | 59 | StepCounter(BLE &ble, events::EventQueue &event_queue) : |
nikoletakuneva | 4:83c7e402fff1 | 60 | _ble(ble), |
nikoletakuneva | 4:83c7e402fff1 | 61 | _event_queue(event_queue), |
nikoletakuneva | 4:83c7e402fff1 | 62 | _step_counter_uuid(STEP_COUNTER_SERVICE_UUID), |
nikoletakuneva | 4:83c7e402fff1 | 63 | _adv_data_builder(_adv_buffer) { } |
nikoletakuneva | 4:83c7e402fff1 | 64 | |
nikoletakuneva | 4:83c7e402fff1 | 65 | void start() { |
nikoletakuneva | 4:83c7e402fff1 | 66 | _ble.gap().setEventHandler(this); |
nikoletakuneva | 4:83c7e402fff1 | 67 | |
nikoletakuneva | 4:83c7e402fff1 | 68 | _ble.init(this, &StepCounter::on_init_complete); |
nikoletakuneva | 4:83c7e402fff1 | 69 | |
nikoletakuneva | 4:83c7e402fff1 | 70 | _event_queue.call_every(500, this, &StepCounter::blink); |
nikoletakuneva | 4:83c7e402fff1 | 71 | _event_queue.call_every(3000, this, &StepCounter::update_step_count); |
nikoletakuneva | 4:83c7e402fff1 | 72 | |
nikoletakuneva | 4:83c7e402fff1 | 73 | t.start(callback(&_event_queue, &EventQueue::dispatch_forever)); |
nikoletakuneva | 4:83c7e402fff1 | 74 | } |
nikoletakuneva | 4:83c7e402fff1 | 75 | void update_step_count() { |
nikoletakuneva | 4:83c7e402fff1 | 76 | printf("updating step count %d\n\r", step_count); |
nikoletakuneva | 4:83c7e402fff1 | 77 | if (_ble.gap().getState().connected) { |
nikoletakuneva | 4:83c7e402fff1 | 78 | _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int)); |
nikoletakuneva | 4:83c7e402fff1 | 79 | } |
nikoletakuneva | 4:83c7e402fff1 | 80 | } |
nikoletakuneva | 4:83c7e402fff1 | 81 | |
nikoletakuneva | 4:83c7e402fff1 | 82 | private: |
nikoletakuneva | 4:83c7e402fff1 | 83 | /** Callback triggered when the ble initialization process has finished */ |
nikoletakuneva | 4:83c7e402fff1 | 84 | void on_init_complete(BLE::InitializationCompleteCallbackContext *params) { |
nikoletakuneva | 4:83c7e402fff1 | 85 | printf("on init complete\n\r"); |
nikoletakuneva | 4:83c7e402fff1 | 86 | if (params->error != BLE_ERROR_NONE) { |
nikoletakuneva | 4:83c7e402fff1 | 87 | print_error(params->error, "Ble initialization failed."); |
nikoletakuneva | 4:83c7e402fff1 | 88 | return; |
nikoletakuneva | 4:83c7e402fff1 | 89 | } |
nikoletakuneva | 4:83c7e402fff1 | 90 | |
nikoletakuneva | 4:83c7e402fff1 | 91 | _ble.gattServer().onDataWritten(this, &StepCounter::on_data_written); |
nikoletakuneva | 4:83c7e402fff1 | 92 | |
nikoletakuneva | 4:83c7e402fff1 | 93 | print_mac_address(); |
nikoletakuneva | 4:83c7e402fff1 | 94 | |
nikoletakuneva | 4:83c7e402fff1 | 95 | start_advertising(); |
nikoletakuneva | 4:83c7e402fff1 | 96 | } |
nikoletakuneva | 4:83c7e402fff1 | 97 | |
nikoletakuneva | 4:83c7e402fff1 | 98 | void start_advertising() { |
nikoletakuneva | 4:83c7e402fff1 | 99 | printf("start advertising\n\r"); |
nikoletakuneva | 4:83c7e402fff1 | 100 | /* Create advertising parameters and payload */ |
nikoletakuneva | 4:83c7e402fff1 | 101 | |
nikoletakuneva | 4:83c7e402fff1 | 102 | ble::AdvertisingParameters adv_parameters( |
nikoletakuneva | 4:83c7e402fff1 | 103 | ble::advertising_type_t::CONNECTABLE_UNDIRECTED, |
nikoletakuneva | 4:83c7e402fff1 | 104 | ble::adv_interval_t(ble::millisecond_t(1000)) |
nikoletakuneva | 4:83c7e402fff1 | 105 | ); |
nikoletakuneva | 4:83c7e402fff1 | 106 | |
nikoletakuneva | 4:83c7e402fff1 | 107 | _adv_data_builder.setFlags(); |
nikoletakuneva | 4:83c7e402fff1 | 108 | _adv_data_builder.setLocalServiceList(mbed::make_Span(&_step_counter_uuid, 1)); |
nikoletakuneva | 4:83c7e402fff1 | 109 | _adv_data_builder.setName(DEVICE_NAME); |
nikoletakuneva | 4:83c7e402fff1 | 110 | |
nikoletakuneva | 4:83c7e402fff1 | 111 | /* Setup advertising */ |
nikoletakuneva | 4:83c7e402fff1 | 112 | |
nikoletakuneva | 4:83c7e402fff1 | 113 | ble_error_t error = _ble.gap().setAdvertisingParameters( |
nikoletakuneva | 4:83c7e402fff1 | 114 | ble::LEGACY_ADVERTISING_HANDLE, |
nikoletakuneva | 4:83c7e402fff1 | 115 | adv_parameters |
nikoletakuneva | 4:83c7e402fff1 | 116 | ); |
nikoletakuneva | 4:83c7e402fff1 | 117 | |
nikoletakuneva | 4:83c7e402fff1 | 118 | if (error) { |
nikoletakuneva | 4:83c7e402fff1 | 119 | print_error(error, "_ble.gap().setAdvertisingParameters() failed"); |
nikoletakuneva | 4:83c7e402fff1 | 120 | return; |
nikoletakuneva | 4:83c7e402fff1 | 121 | } |
nikoletakuneva | 4:83c7e402fff1 | 122 | |
nikoletakuneva | 4:83c7e402fff1 | 123 | error = _ble.gap().setAdvertisingPayload( |
nikoletakuneva | 4:83c7e402fff1 | 124 | ble::LEGACY_ADVERTISING_HANDLE, |
nikoletakuneva | 4:83c7e402fff1 | 125 | _adv_data_builder.getAdvertisingData() |
nikoletakuneva | 4:83c7e402fff1 | 126 | ); |
nikoletakuneva | 4:83c7e402fff1 | 127 | |
nikoletakuneva | 4:83c7e402fff1 | 128 | if (error) { |
nikoletakuneva | 4:83c7e402fff1 | 129 | print_error(error, "_ble.gap().setAdvertisingPayload() failed"); |
nikoletakuneva | 4:83c7e402fff1 | 130 | return; |
nikoletakuneva | 4:83c7e402fff1 | 131 | } |
nikoletakuneva | 4:83c7e402fff1 | 132 | |
nikoletakuneva | 4:83c7e402fff1 | 133 | /* Start advertising */ |
nikoletakuneva | 4:83c7e402fff1 | 134 | |
nikoletakuneva | 4:83c7e402fff1 | 135 | error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE); |
nikoletakuneva | 4:83c7e402fff1 | 136 | |
nikoletakuneva | 4:83c7e402fff1 | 137 | if (error) { |
nikoletakuneva | 4:83c7e402fff1 | 138 | print_error(error, "_ble.gap().startAdvertising() failed"); |
nikoletakuneva | 4:83c7e402fff1 | 139 | return; |
nikoletakuneva | 4:83c7e402fff1 | 140 | } |
nikoletakuneva | 4:83c7e402fff1 | 141 | } |
nikoletakuneva | 4:83c7e402fff1 | 142 | |
nikoletakuneva | 4:83c7e402fff1 | 143 | void on_data_written(const GattWriteCallbackParams *params) { |
nikoletakuneva | 4:83c7e402fff1 | 144 | if ((params->handle == step_count_state.getValueHandle()) && (params->len == 1)) { |
nikoletakuneva | 4:83c7e402fff1 | 145 | step_count = *(params->data); |
nikoletakuneva | 4:83c7e402fff1 | 146 | } |
nikoletakuneva | 4:83c7e402fff1 | 147 | step_count = 0; |
nikoletakuneva | 4:83c7e402fff1 | 148 | } |
nikoletakuneva | 4:83c7e402fff1 | 149 | |
nikoletakuneva | 4:83c7e402fff1 | 150 | void blink(void) { |
nikoletakuneva | 4:83c7e402fff1 | 151 | led1 = !led1; |
nikoletakuneva | 4:83c7e402fff1 | 152 | } |
nikoletakuneva | 4:83c7e402fff1 | 153 | |
nikoletakuneva | 4:83c7e402fff1 | 154 | private: |
nikoletakuneva | 4:83c7e402fff1 | 155 | /* Event handler */ |
nikoletakuneva | 4:83c7e402fff1 | 156 | |
nikoletakuneva | 4:83c7e402fff1 | 157 | void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) { |
nikoletakuneva | 4:83c7e402fff1 | 158 | _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE); |
nikoletakuneva | 4:83c7e402fff1 | 159 | } |
nikoletakuneva | 4:83c7e402fff1 | 160 | |
nikoletakuneva | 4:83c7e402fff1 | 161 | private: |
nikoletakuneva | 4:83c7e402fff1 | 162 | BLE &_ble; |
nikoletakuneva | 4:83c7e402fff1 | 163 | events::EventQueue &_event_queue; |
nikoletakuneva | 4:83c7e402fff1 | 164 | |
nikoletakuneva | 4:83c7e402fff1 | 165 | UUID _step_counter_uuid; |
nikoletakuneva | 4:83c7e402fff1 | 166 | |
nikoletakuneva | 4:83c7e402fff1 | 167 | uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE]; |
nikoletakuneva | 4:83c7e402fff1 | 168 | ble::AdvertisingDataBuilder _adv_data_builder; |
nikoletakuneva | 4:83c7e402fff1 | 169 | }; |
nikoletakuneva | 4:83c7e402fff1 | 170 | |
nikoletakuneva | 4:83c7e402fff1 | 171 | /** Schedule processing of events from the BLE middleware in the event queue. */ |
nikoletakuneva | 4:83c7e402fff1 | 172 | void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) { |
nikoletakuneva | 4:83c7e402fff1 | 173 | event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents)); |
nikoletakuneva | 4:83c7e402fff1 | 174 | } |
nikoletakuneva | 4:83c7e402fff1 | 175 | |
nikoletakuneva | 4:83c7e402fff1 | 176 | |
castlefei | 0:44701eab0261 | 177 | // main() runs in its own thread in the OS |
castlefei | 0:44701eab0261 | 178 | int main() |
castlefei | 0:44701eab0261 | 179 | { |
nikoletakuneva | 4:83c7e402fff1 | 180 | BLE &ble = BLE::Instance(); |
nikoletakuneva | 4:83c7e402fff1 | 181 | |
nikoletakuneva | 4:83c7e402fff1 | 182 | GattCharacteristic *charTable[] = {&step_count_state}; |
nikoletakuneva | 4:83c7e402fff1 | 183 | GattService step_count_service(STEP_COUNTER_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *)); |
nikoletakuneva | 4:83c7e402fff1 | 184 | ble.addService(step_count_service); |
nikoletakuneva | 4:83c7e402fff1 | 185 | |
nikoletakuneva | 4:83c7e402fff1 | 186 | ble.onEventsToProcess(schedule_ble_events); |
nikoletakuneva | 4:83c7e402fff1 | 187 | |
nikoletakuneva | 4:83c7e402fff1 | 188 | StepCounter demo(ble, event_queue); |
nikoletakuneva | 4:83c7e402fff1 | 189 | demo.start(); |
nikoletakuneva | 4:83c7e402fff1 | 190 | |
castlefei | 0:44701eab0261 | 191 | //new mpu(data,clk,address),in constructor addr7bit<<1 |
castlefei | 0:44701eab0261 | 192 | mpu9250 *mpu = new mpu9250(p26,p27,addr7bit); |
castlefei | 0:44701eab0261 | 193 | //scale of acc and gyro |
castlefei | 0:44701eab0261 | 194 | mpu->initMPU9250(0x00,0x00); |
castlefei | 0:44701eab0261 | 195 | |
castlefei | 0:44701eab0261 | 196 | float AccRead[3]; |
castlefei | 0:44701eab0261 | 197 | float GyroRead[3]; |
castlefei | 0:44701eab0261 | 198 | float TempRead[1]; |
castlefei | 2:d4c480d17944 | 199 | float res_smooth; |
castlefei | 2:d4c480d17944 | 200 | //vector<float> res_list; |
nikoletakuneva | 4:83c7e402fff1 | 201 | float threshold=0.1; |
nikoletakuneva | 4:83c7e402fff1 | 202 | int number=0; |
nikoletakuneva | 4:83c7e402fff1 | 203 | int numpeak = 0; |
castlefei | 1:92f42e198925 | 204 | |
castlefei | 2:d4c480d17944 | 205 | static MatrixXd acc_raw(3,0); |
castlefei | 1:92f42e198925 | 206 | Vector3d acc_new; |
castlefei | 1:92f42e198925 | 207 | MatrixXd C; |
castlefei | 1:92f42e198925 | 208 | MatrixXd vec, val; |
castlefei | 1:92f42e198925 | 209 | int dim = 1; //dimension of PCA |
castlefei | 2:d4c480d17944 | 210 | //use the class defined in pca.h and peak.h |
castlefei | 2:d4c480d17944 | 211 | PCA pca; |
castlefei | 2:d4c480d17944 | 212 | PEAK peak; |
castlefei | 2:d4c480d17944 | 213 | |
castlefei | 2:d4c480d17944 | 214 | bqc.add(&mybq); |
castlefei | 2:d4c480d17944 | 215 | |
castlefei | 2:d4c480d17944 | 216 | static vector<float> res_list; |
castlefei | 1:92f42e198925 | 217 | |
castlefei | 0:44701eab0261 | 218 | while (true) { |
castlefei | 0:44701eab0261 | 219 | |
castlefei | 2:d4c480d17944 | 220 | //when i add to 600, there accure faugment error |
castlefei | 2:d4c480d17944 | 221 | //printf("the %f loop", number); |
castlefei | 2:d4c480d17944 | 222 | number = number +1; |
castlefei | 2:d4c480d17944 | 223 | //vector<float> res_list; |
castlefei | 0:44701eab0261 | 224 | //Blink LED and wait 1 seconds |
nikoletakuneva | 4:83c7e402fff1 | 225 | //led1 = !led1; |
nikoletakuneva | 4:83c7e402fff1 | 226 | //thread_sleep_for(SLEEP_TIME); |
castlefei | 0:44701eab0261 | 227 | //read and convert date |
castlefei | 0:44701eab0261 | 228 | mpu->ReadConvertAll(AccRead,GyroRead,TempRead); |
castlefei | 2:d4c480d17944 | 229 | AccRead[0]= AccRead[0]/1000; |
castlefei | 2:d4c480d17944 | 230 | AccRead[1]= AccRead[1]/1000; |
castlefei | 2:d4c480d17944 | 231 | AccRead[2]= AccRead[2]/1000; |
castlefei | 0:44701eab0261 | 232 | printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]); |
castlefei | 2:d4c480d17944 | 233 | //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]); |
castlefei | 2:d4c480d17944 | 234 | //printf("temp value is %f.\n\r",TempRead[0]); |
castlefei | 0:44701eab0261 | 235 | |
castlefei | 1:92f42e198925 | 236 | //append new data to matrix acc_raw |
castlefei | 1:92f42e198925 | 237 | //adding the columns |
castlefei | 1:92f42e198925 | 238 | acc_new << AccRead[0],AccRead[1],AccRead[2]; |
castlefei | 1:92f42e198925 | 239 | acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1); |
castlefei | 1:92f42e198925 | 240 | acc_raw.col(acc_raw.cols()-1) = acc_new; |
nikoletakuneva | 4:83c7e402fff1 | 241 | printf("before if\n\r"); |
nikoletakuneva | 4:83c7e402fff1 | 242 | printf("number: %d\n\r", number); |
nikoletakuneva | 4:83c7e402fff1 | 243 | if(number % 10 ==2) |
castlefei | 2:d4c480d17944 | 244 | { |
nikoletakuneva | 4:83c7e402fff1 | 245 | printf("in if\n\r"); |
nikoletakuneva | 4:83c7e402fff1 | 246 | if(number > 2) |
nikoletakuneva | 4:83c7e402fff1 | 247 | { |
nikoletakuneva | 4:83c7e402fff1 | 248 | //cout << acc_raw << endl; |
nikoletakuneva | 4:83c7e402fff1 | 249 | //run PCA |
nikoletakuneva | 4:83c7e402fff1 | 250 | MatrixXd X1=pca.featurnormail(acc_raw); |
nikoletakuneva | 4:83c7e402fff1 | 251 | pca.ComComputeCov(X1, C); |
nikoletakuneva | 4:83c7e402fff1 | 252 | pca.ComputEig(C, vec, val); |
nikoletakuneva | 4:83c7e402fff1 | 253 | //select dim num of eigenvector from right to left. right is important |
nikoletakuneva | 4:83c7e402fff1 | 254 | //compute the result array |
nikoletakuneva | 4:83c7e402fff1 | 255 | MatrixXd res = vec.rightCols(dim).transpose()*X1; |
nikoletakuneva | 4:83c7e402fff1 | 256 | |
nikoletakuneva | 4:83c7e402fff1 | 257 | //show the result after PCA |
nikoletakuneva | 4:83c7e402fff1 | 258 | //////cout << "result" << res << endl; |
nikoletakuneva | 4:83c7e402fff1 | 259 | vector<float> res_list={}; |
nikoletakuneva | 4:83c7e402fff1 | 260 | |
nikoletakuneva | 4:83c7e402fff1 | 261 | //printf("result of PCA size:%d\n\r",res.cols()); |
nikoletakuneva | 4:83c7e402fff1 | 262 | for(int i = 0; i < res.cols(); i++) |
nikoletakuneva | 4:83c7e402fff1 | 263 | { |
nikoletakuneva | 4:83c7e402fff1 | 264 | res_smooth = bqc.step(res(i)); |
nikoletakuneva | 4:83c7e402fff1 | 265 | res_list.push_back(res_smooth); |
nikoletakuneva | 4:83c7e402fff1 | 266 | //printf("result after filter in for loop %d: %f\n\r",i,res_smooth); |
nikoletakuneva | 4:83c7e402fff1 | 267 | //std::cout << "\t" << bqc.step( ) << std::endl; |
nikoletakuneva | 4:83c7e402fff1 | 268 | } |
nikoletakuneva | 4:83c7e402fff1 | 269 | int len = res_list.size(); |
nikoletakuneva | 4:83c7e402fff1 | 270 | //printf("len of res:%d\n\r", len); |
nikoletakuneva | 4:83c7e402fff1 | 271 | numpeak = peak.findPeaks(res_list,len,threshold); |
nikoletakuneva | 4:83c7e402fff1 | 272 | printf("NUMBER OF PEAKS: %d\n\r", numpeak); |
nikoletakuneva | 4:83c7e402fff1 | 273 | //printf("height in main: %f\n\r", threshold); |
nikoletakuneva | 4:83c7e402fff1 | 274 | step_count += numpeak; |
nikoletakuneva | 4:83c7e402fff1 | 275 | printf("num of step: %d\n\r", step_count); |
nikoletakuneva | 4:83c7e402fff1 | 276 | |
nikoletakuneva | 4:83c7e402fff1 | 277 | //clear the matrix to contain new data |
nikoletakuneva | 4:83c7e402fff1 | 278 | acc_raw.conservativeResize(3, 0); |
nikoletakuneva | 4:83c7e402fff1 | 279 | } |
castlefei | 2:d4c480d17944 | 280 | } |
nikoletakuneva | 4:83c7e402fff1 | 281 | number = number +1; |
castlefei | 0:44701eab0261 | 282 | } |
castlefei | 0:44701eab0261 | 283 | } |