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

Integration of code (not working)

Dependencies:   mpu9250_i2c biquadFilter PCA peakdetection Eigen

main.cpp@5:8c276289611c, 2019-11-28 (annotated)

Committer:
nikoletakuneva
Date:
Thu Nov 28 17:56:09 2019 +0000
Revision:
5:8c276289611c
Parent:
4:83c7e402fff1
Child:
6:4437975b9cf9
Latest code

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