Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerômetro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.
Dependencies: mbed MatrixMath Matrix nrf51_rtc BMP180 MPU9250
Diff: findPeaks.h
- Revision:
- 5:d87c25f009d1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/findPeaks.h Sat Jul 27 12:17:25 2019 +0000 @@ -0,0 +1,214 @@ +// INPUTS: +// Signal - A real vector from the maxima will be found (required) +// sel - The amount above surrounding data for a peak to be, +// identified (default = (max(x0)-min(x0))/4). Larger values mean +// the algorithm is more selective in finding peaks. +// thresh - A threshold value which peaks must be larger than to be +// maxima or smaller than to be minima. +int iiSearch=0; +#define SearchSize 60 +float Signal[SearchSize]; +float Threshold; +int steps=0; + +void diff(float (*x0)[SearchSize], float (*dx0)[SearchSize]) +{ + for(int ii=1; ii<SearchSize; ii=ii+1) + { + (*dx0)[ii-1] = (*x0)[ii] - (*x0)[ii-1]; + } + +} +int findSig(float (*dx0)[SearchSize], int (*ind)[SearchSize]) +{ + int jj=0; + for(int ii=1; ii<SearchSize; ii=ii+1) + { + if ((*dx0)[ii]>0) + { + if((*dx0)[ii-1]<0) + { + (*ind)[jj]=ii; + jj=jj+1; + } + } + else + { + if((*dx0)[ii-1]>0) + { + (*ind)[jj]=ii; + jj=jj+1; + } + } + } + return jj; +} + +int findPeaks(float Data, float sel, float thresh) //Signal /Sel/ Theshoold +{ + if (iiSearch>=SearchSize) + { + for (int jjSearch=0;jjSearch<=SearchSize-2;jjSearch=jjSearch+1) + { + Signal[jjSearch]=Signal[jjSearch+1]; + } + Signal[SearchSize-1]=Data; + + + float (dx0)[SearchSize]; + diff(&Signal,&dx0); //Find derivative + + for (int ii=0; ii<SearchSize ; ii=ii+1) + { + if ((dx0)[ii]==0) + { + (dx0)[ii]=2.22044604925031e-16; + } + } + int pointsFound=0; + int ind[SearchSize]; + pointsFound=findSig(&dx0,&ind); + if (pointsFound==0) + { + return 0; + } + float x[pointsFound]; + for (int ii=0; ii<pointsFound; ii=ii+1) + { + int aux=ind[ii]; + x[ii]=(Signal)[aux]; + } + //finding the minimal value + float minMag=x[0]; + for (int ii=1; ii<pointsFound; ii=ii+1) + { + if (minMag>x[ii]) + { + minMag=x[ii]; + } + } + float leftMin; + if (x[0]>(Signal)[0]) + { + leftMin = (Signal)[0]; + } + else + { + leftMin = x[0]; + } + + //struct Peaks PeaksFound; + int PeakCount=0; + + // x only has the peaks, valleys, and possibly endpoints + if (pointsFound > 2) + { + float tempMag=minMag; + int tempLoc; + int foundPeak=0; + //Skip the first point if it is smaller so we always start on a maxima + int xx; + if (x[0] >= x[1]) + { + xx=-1; + } + else + { + xx=0; + } + + //Loop through extrema which should be peaks and then valleys + while (xx<pointsFound-1) + { + xx=xx+1; //This is a peak + //Reset peak finding if we had a peak and the next peak is bigger + //than the last or the left min was small enough to reset. + if (foundPeak==1) + { + tempMag=minMag; + foundPeak=0; + } + //Found new peak that was lager than temp mag and selectivity larger + //than the minimum to its left. + if (x[xx] > tempMag && x[xx] > leftMin + sel && x[xx]>thresh) + { + tempLoc=xx; + tempMag=x[xx]; + } + //Make sure we don't iterate past the length of our vector + if (xx==(pointsFound-1)) + { + break; + } + xx=xx+1; //Move onto the valley + //Come down at least sel from peak && x[xx]>thresh && tempMag>thresh && tempMag>thresh && (Signal)[ii]>thresh + if (foundPeak==0 && tempMag > sel+x[xx] && tempMag>thresh) + { + foundPeak=1; + leftMin=x[xx]; + PeakCount=PeakCount+1; + } + else if (x[xx]<leftMin) + { + leftMin=x[xx]; + } + } + if (foundPeak==0) + { + float auxmin; //find min between min(x0(end), x(end)) + if ((Signal)[SearchSize-1]<x[pointsFound-1]) + { + auxmin=(Signal)[SearchSize-1]; + } + else + { + auxmin=x[pointsFound-1]; + } + + if (x[pointsFound-1] > tempMag && x[pointsFound-1] > leftMin + sel ) + { + PeakCount=PeakCount+1; + } + else if (tempMag > auxmin + sel && tempMag>thresh) + { + PeakCount=PeakCount+1; + } + } + + } + else + { + float auxMaxMag; + if (pointsFound==2) + { + int auxloc1=ind[0]; + int auxloc2=ind[1]; + if ((Signal)[auxloc1] > (Signal)[auxloc2] ) + { + auxMaxMag=(Signal)[auxloc1]; + } + else + { + auxMaxMag=(Signal)[auxloc2]; + } + } + else + { + int auxloc1=ind[0]; + auxMaxMag=(Signal)[auxloc1]; + } + + if (auxMaxMag>=thresh) + { + PeakCount=1; + } + } + return PeakCount; + } + else + { + Signal[iiSearch]=Data; + iiSearch=iiSearch+1; + return 0; + } +} \ No newline at end of file