Nikoleta Kuneva / Mbed OS mbed-test-i2c-PCA-biquad-peakdet

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?

UserRevisionLine numberNew 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 }