wenchang fei / Mbed OS mbed-os-step-counting

mix code vision 3. Using the previous algorithm to detect peaks as Nikoleta and Shiyao. Adding overlapping windows

Dependencies:   mpu9250_i2c biquadFilter peakdetection Eigen

main.cpp@2:d4c480d17944, 2019-11-25 (annotated)

Committer:
castlefei
Date:
Mon Nov 25 14:28:46 2019 +0000
Revision:
2:d4c480d17944
Parent:
1:92f42e198925
Child:
4:15d6c7123b09
the test code including reading data form MPU9250, reducing dimension, smoothing, detecting peaks and printing the steps.

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
castlefei 0:44701eab0261 13 #include "mbed.h"
castlefei 0:44701eab0261 14 #include "platform/mbed_thread.h"
castlefei 0:44701eab0261 15 #include "stats_report.h"
castlefei 0:44701eab0261 16 #include "MPU9250.h"
castlefei 2:d4c480d17944 17 //#include <Eigen/Dense.h>
castlefei 1:92f42e198925 18 #include <iostream>
castlefei 2:d4c480d17944 19 #include <vector>
castlefei 2:d4c480d17944 20 #include <complex>
castlefei 2:d4c480d17944 21 #include "BiQuad.h"
castlefei 2:d4c480d17944 22 #include "pca.h"
castlefei 2:d4c480d17944 23 #include "peak.h"
castlefei 1:92f42e198925 24
castlefei 1:92f42e198925 25 using namespace std;
castlefei 1:92f42e198925 26 using namespace Eigen;
castlefei 0:44701eab0261 27
castlefei 0:44701eab0261 28 DigitalOut led1(LED1);
castlefei 0:44701eab0261 29 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
castlefei 2:d4c480d17944 30 //the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719
castlefei 2:d4c480d17944 31 BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
castlefei 2:d4c480d17944 32 BiQuadChain bqc;
castlefei 1:92f42e198925 33
castlefei 1:92f42e198925 34
castlefei 2:d4c480d17944 35 #define SLEEP_TIME 20 // (msec)
castlefei 1:92f42e198925 36
castlefei 0:44701eab0261 37
castlefei 0:44701eab0261 38 // main() runs in its own thread in the OS
castlefei 0:44701eab0261 39 int main()
castlefei 0:44701eab0261 40 {
castlefei 0:44701eab0261 41 //new mpu(data,clk,address),in constructor addr7bit<<1
castlefei 0:44701eab0261 42 mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
castlefei 0:44701eab0261 43 //scale of acc and gyro
castlefei 0:44701eab0261 44 mpu->initMPU9250(0x00,0x00);
castlefei 0:44701eab0261 45
castlefei 0:44701eab0261 46 float AccRead[3];
castlefei 0:44701eab0261 47 float GyroRead[3];
castlefei 0:44701eab0261 48 float TempRead[1];
castlefei 2:d4c480d17944 49 float res_smooth;
castlefei 2:d4c480d17944 50 //vector<float> res_list;
castlefei 2:d4c480d17944 51 float number=0;
castlefei 1:92f42e198925 52
castlefei 2:d4c480d17944 53 static MatrixXd acc_raw(3,0);
castlefei 1:92f42e198925 54 Vector3d acc_new;
castlefei 1:92f42e198925 55 MatrixXd C;
castlefei 1:92f42e198925 56 MatrixXd vec, val;
castlefei 1:92f42e198925 57 int dim = 1; //dimension of PCA
castlefei 2:d4c480d17944 58 //use the class defined in pca.h and peak.h
castlefei 2:d4c480d17944 59 PCA pca;
castlefei 2:d4c480d17944 60 PEAK peak;
castlefei 2:d4c480d17944 61
castlefei 2:d4c480d17944 62 bqc.add(&mybq);
castlefei 2:d4c480d17944 63
castlefei 2:d4c480d17944 64 static vector<float> res_list;
castlefei 1:92f42e198925 65
castlefei 0:44701eab0261 66 while (true) {
castlefei 0:44701eab0261 67
castlefei 2:d4c480d17944 68 //when i add to 600, there accure faugment error
castlefei 2:d4c480d17944 69 //printf("the %f loop", number);
castlefei 2:d4c480d17944 70 number = number +1;
castlefei 2:d4c480d17944 71 //vector<float> res_list;
castlefei 0:44701eab0261 72 //Blink LED and wait 1 seconds
castlefei 0:44701eab0261 73 led1 = !led1;
castlefei 0:44701eab0261 74 thread_sleep_for(SLEEP_TIME);
castlefei 0:44701eab0261 75 //read and convert date
castlefei 0:44701eab0261 76 mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
castlefei 2:d4c480d17944 77 AccRead[0]= AccRead[0]/1000;
castlefei 2:d4c480d17944 78 AccRead[1]= AccRead[1]/1000;
castlefei 2:d4c480d17944 79 AccRead[2]= AccRead[2]/1000;
castlefei 0:44701eab0261 80 printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
castlefei 2:d4c480d17944 81 //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
castlefei 2:d4c480d17944 82 //printf("temp value is %f.\n\r",TempRead[0]);
castlefei 0:44701eab0261 83
castlefei 1:92f42e198925 84 //append new data to matrix acc_raw
castlefei 1:92f42e198925 85 //adding the columns
castlefei 1:92f42e198925 86 acc_new << AccRead[0],AccRead[1],AccRead[2];
castlefei 1:92f42e198925 87 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
castlefei 1:92f42e198925 88 acc_raw.col(acc_raw.cols()-1) = acc_new;
castlefei 1:92f42e198925 89
castlefei 2:d4c480d17944 90 //////cout << "acc_raw:" << acc_raw << endl;
castlefei 1:92f42e198925 91
castlefei 1:92f42e198925 92 //run PCA
castlefei 2:d4c480d17944 93 MatrixXd X1=pca.featurnormail(acc_raw);
castlefei 2:d4c480d17944 94 pca.ComComputeCov(X1, C);
castlefei 2:d4c480d17944 95 pca.ComputEig(C, vec, val);
castlefei 1:92f42e198925 96 //select dim num of eigenvector from right to left. right is important
castlefei 1:92f42e198925 97 //compute the result array
castlefei 1:92f42e198925 98 MatrixXd res = vec.rightCols(dim).transpose()*X1;
castlefei 1:92f42e198925 99
castlefei 1:92f42e198925 100 //show the result after PCA
castlefei 2:d4c480d17944 101 //////cout << "result" << res << endl;
castlefei 2:d4c480d17944 102 res_list.clear();
castlefei 2:d4c480d17944 103 for(int i = 0; i < res.cols(); i++)
castlefei 2:d4c480d17944 104 {
castlefei 2:d4c480d17944 105 res_smooth = bqc.step(res(i));
castlefei 2:d4c480d17944 106 res_list.push_back(res_smooth);
castlefei 2:d4c480d17944 107 //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
castlefei 2:d4c480d17944 108 //std::cout << "\t" << bqc.step( ) << std::endl;
castlefei 2:d4c480d17944 109 }
castlefei 2:d4c480d17944 110 int len = res_list.size();
castlefei 2:d4c480d17944 111 printf("len of res:%d\n\r", len);
castlefei 2:d4c480d17944 112 peak.findPeaks(res_list,len,0.1);
castlefei 2:d4c480d17944 113
castlefei 2:d4c480d17944 114
castlefei 0:44701eab0261 115 }
castlefei 0:44701eab0261 116 }