int blockLowPass2Test()
{
printf("Test BlockLowPass2\t\t: ");
BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
// test initial state
ASSERT(equal(10.0f, lowPass.getFCutParam()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
/*
Once we have the subtitle built, we do the u& v planes
and some smoothing to avoid over sharpening on the chroma plane
*/
uint8_t ADMVideoSubtitle::doChroma(void)
{
// now blur bitmap into mask..
memset(_maskBuffer,0,SRT_MAX_LINE*_conf->_fontsize*_info.width);
uint32_t off;
uint8_t *src,*dst;
off=0;
src=_bitmapBuffer;
dst=_maskBuffer;
// We shrink it down for u & v by 2x2
// mask buffer->bitmap buffer
uint8_t tmp[_info.width*_info.height];
decimate(src,tmp,_info.width,_info.height);
lowPass(src,dst,_info.width,_info.height);
lowPass(tmp,src,_info.width>>1,_info.height>>1);
if (_conf->_useBackgroundColor)
{
decimate(_bgMaskBuffer,_bgBitmapBuffer,_info.width,_info.height);
//lowPass(tmp,_bgBitmapBuffer,_info.width>>1,_info.height>>1);
}
}
void randist_bang(t_randist *x)
{
long possNum = x->possNum;
float var1 = x->varFlt1;
float var2 = x->varFlt2;
// post("var1 = lf1 /n var2 = %f2", var1,var2);
//Select which function to execute
switch (x->distType) {
case 0:
x->randVar = equalDist(x,possNum);
break;
case 1:
x->randVar = highPass(possNum);
break;
case 2:
x->randVar = lowPass(possNum);
break;
case 3:
x->randVar = triangle(possNum);
break;
case 4:
x->randVar = gaussian(possNum, var1, var2);
break;
case 5:
x->randVar = exponential(possNum, var2);
break;
default:
post("error with distType");
break;
}
outlet_float(x->rand_out, x->randVar);
}
// function containing the filtering of the data
int filter() {
insert(&buffered_raw_data, getNextData());
insert(&buffered_LP_data, lowPass(&buffered_raw_data, &buffered_LP_data));
insert(&buffered_HP_data, highPass(&buffered_LP_data, &buffered_HP_data));
insert(&buffered_derivative_square_data,
derivative_square(&buffered_HP_data));
return movingWindowIntegration(&buffered_derivative_square_data);
}
//
// Display up to 3 lines of text centered on screen
// Each line is separated by a |
//______________________________________
void ADMVideoSubtitle::displayString(char *string)
{
uint32_t base,last=0,line=0;
double bbase;
// printf("%s\n",string);
// bbase is the final position in line
// in the image
base=0;
memset(_bitmapBuffer,0,_info.height*_info.width);
memset(_maskBuffer,0,_info.height*_info.width);
// search for | char
for(uint32_t i=0;i<strlen(string);i++)
{
if( *(string+i)=='|')
{
displayLine(string+last,base,i-last);
last=i+1;
base+=_conf->_fontsize;
line++;
if(line>3)
{
printf("\n Maximum 3 lines!\n");
return;
}
}
}
//printf("\len :%d\n",strlen(string)-last);
displayLine(string+last,base,strlen(string)-last);
// now blur bitmap into mask..
memset(_maskBuffer,0,SRT_MAX_LINE*_conf->_fontsize*_info.width);
uint32_t off;
uint8_t *src,*dst;
off=0;
src=_bitmapBuffer;
dst=_maskBuffer;
// We shrink it down for u & v by 2x2
// mask buffer->bitmap buffer
uint8_t tmp[_info.width*_info.height];
decimate(src,tmp,_info.width,_info.height);
lowPass(src,dst,_info.width,_info.height);
lowPass(tmp,src,_info.width>>1,_info.height>>1);
}
void ActorHighLevel::slotUpdateWaypoints(QList< QPair<double,double> > waypoints)
{
// signals will be ignored, if quitting is true
if (!quitting)
{
//wenn ein anderer Status activ ist, soll dieser nicht gestört werden
switch(this->getState()) {
case StatePathProcessing::STOP:
case StatePathProcessing::RUNNING:
{
internalWP = waypoints;
numWP = internalWP.length();
// Anhalten, falls es nicht genug Wegpunkte gibt
if(numWP == 0) {
this->setState(StatePathProcessing::STOP);
enabled = true;
this->startPIDController();
return;
}
else {
this->setState(StatePathProcessing::RUNNING);
}
}
break;
default: // jeder andere Status soll beendet werden
{
// nichts machen!
return;
} // default
} // switch
// Ein Spline mit zwei Stützpunkten kann nicht generiert werden, schummele einen mittleren dazu.
if(numWP == 1){
double midX = (internalWP.first().first + internalWP.last().first )/2;
double midY = (internalWP.first().second + internalWP.last().second)/2;
internalWP.insert(1, QPair<double,double> (midX, midY));
++numWP;
}
// Wegpunkte Tiefpassfiltern
QVector<double> wpLowPassX = lowPass(takeDimension(internalWP.toVector(), 0), config::actorWPLowPassAlpha);
QVector<double> wpLowPassY = lowPass(takeDimension(internalWP.toVector(), 1), config::actorWPLowPassAlpha);
// Spline Metrik erzeugen
QVector<double> splineMetric(numWP);
for(int i=0; i<numWP; ++i){
splineMetric[i] = static_cast<double>(i);
}
// Spline generieren
splineX.set_points(splineMetric, wpLowPassX);
splineY.set_points(splineMetric, wpLowPassY);
// Send generated spline to path display tab
if(PathPlanning::streamPathEnabled) {
static const int numSplinePoints = 100;
PathPlotData dataPacket;
dataPacket.dataType = PathPlotData::SPLINE;
dataPacket.splineLength = numWP;
QVector<double> splineVectX(numSplinePoints), splineVectY(numSplinePoints);
for(int i=0; i<numSplinePoints; i++){
double progress = static_cast<double>(i*dataPacket.splineLength)/static_cast<double>(numSplinePoints-1);
splineVectX[i] = splineX(progress);
splineVectY[i] = splineY(progress);
}
dataPacket.splineX = splineVectX;
dataPacket.splineY = splineVectY;
Q_EMIT signalSplinePlot(dataPacket);
}
// Start timer (verwendet für berechnung der I und D Anteile)
elapsedTime->restart();
// PID wieder starten
enabled = true;
this->startPIDController();
}
}
int main() {
//Two booleans are used when taking calculating the runtime
int clockOn = 0;
int totalClockOn = 1;
//Variables used in calculating runtime
clock_t start, clockBeforeRead, clockAfterRead, clockAfterLowPass,
clockAfterHighPass, clockAfterDer, clockAfterSquare, clockAfterMWI,
clockAfterIdentifyPeaks, end;
double cpuTimeUsed;
double timeReadData = 0;
double timeLowPass = 0;
double timeHighPass = 0;
double timeDerivative = 0;
double timeSquare = 0;
double timeMWindowInt = 0;
double timeIdentifyPeaks = 0;
//The file to read from
static const char filename[] = "ECG.txt";
FILE *file = fopen(filename, "r");
//Arrays and a variable to keep track of the values after each filter
int input[13] = { 0 };
int afterLowpass[33] = { 0 };
int afterHighpass[5] = { 0 };
int afterDerivative = 0;
int afterSquare[30] = { 0 };
int afterWindowIntegration[3] = { 0 };
int value;
int counter = 0;
if (totalClockOn) {
start = clock();
}
//The loops runs as long as it has not reached the end of the file
while (!feof(file)) {
if (clockOn) {
clockBeforeRead = clock();
}
counter++;
//The nex value is saved in the variable "value"
value = getNextData(file);
insertArray(input, 13, value);
/*if (clockOn) {
clockAfterRead = clock();
timeReadData =+ ((double) ((clockAfterRead - clockBeforeRead)) / CLOCKS_PER_SEC ) * 1000;
}*/
lowPass(input, afterLowpass);
/*if (clockOn) {
clockAfterLowPass = clock();
timeLowPass =+ ((double) ((clockAfterLowPass - clockAfterRead)) / CLOCKS_PER_SEC ) * 1000;
}*/
highPass(afterLowpass, afterHighpass);
/*if (clockOn) {
clockAfterHighPass = clock();
timeHighPass =+ ((double) ((clockAfterHighPass - clockAfterLowPass)) / CLOCKS_PER_SEC ) * 1000;
}*/
afterDerivative = derivative(afterHighpass);
/*if (clockOn) {
clockAfterDer = clock();
timeDerivative =+ ((double) ((clockAfterDer - clockAfterHighPass)) / CLOCKS_PER_SEC ) * 1000;
}*/
square(afterDerivative, afterSquare);
/*if (clockOn) {
clockAfterSquare = clock();
timeSquare =+ ((double) ((clockAfterSquare - clockAfterDer)) / CLOCKS_PER_SEC ) * 1000;
}*/
mWindowIntegration(afterSquare, afterWindowIntegration);
if (clockOn) {
clockAfterMWI = clock();
timeMWindowInt = +((double) ((clockAfterMWI - clockBeforeRead))
/ CLOCKS_PER_SEC) * 1000;
}
identifyPeaks(afterWindowIntegration);
if (clockOn) {
clockAfterIdentifyPeaks = clock();
timeIdentifyPeaks = +((double) ((clockAfterIdentifyPeaks
- clockAfterMWI)) / CLOCKS_PER_SEC) * 1000;
}
}
if (totalClockOn) {
end = clock();
cpuTimeUsed = ((double) ((end - start)) / CLOCKS_PER_SEC) * 1000;
printf("Total cpu time: %f milliseconds\n", cpuTimeUsed);
}
if (clockOn) {
//printf("Average time read data: %f nanoseconds\nAverage time Low Pass filter: %f nanoseconds\nAverage time High Pass filter: %f nanoseconds\n", timeReadData, timeLowPass, timeHighPass);
//printf("Average time derivative filter: %f nanoseconds\nAverage time square filter: %f nanoseconds\nAverage time moving window integration filter: %f nanoseconds\n",timeDerivative, timeSquare, timeMWindowInt);
printf("Time to apply filters: %f milliseconds\n", timeMWindowInt);
printf("Time to identify peaks: %f milliseconds\n", timeIdentifyPeaks);
}
return 0;
}
