272 lines
6.6 KiB
C++
272 lines
6.6 KiB
C++
|
#include "peakCentering.h"
|
||
|
|
#include "stdlib.h"
|
||
|
|
|
||
|
|
void peakCentering(
|
||
|
|
std::vector<RgnPix> &InRgnPnt,
|
||
|
|
std::vector<RgnPix> &OutCenteringPnt
|
||
|
|
)
|
||
|
|
{
|
||
|
|
//ap_uint<32> frmNo=0;
|
||
|
|
//ap_uint<32> timeStamp=0;
|
||
|
|
//ap_uint<32> encInfo=0;
|
||
|
|
//ap_uint<16> frmW=0, frmH=0, frmX=0, frmY=0;
|
||
|
|
ushort WinNum = 0;
|
||
|
|
RgnPix InData;
|
||
|
|
int readPtr = 0;
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
uchar VSync = InData.Sync & 0x02;
|
||
|
|
if(0 == VSync) //wait for syncFirst
|
||
|
|
return;
|
||
|
|
|
||
|
|
//1st: frameNo
|
||
|
|
//frmNo(15,0) = InData.winStartX.range(15,0);
|
||
|
|
//frmNo(31,16) = InData.y.range(15,0);
|
||
|
|
OutCenteringPnt.push_back(InData);
|
||
|
|
//2nd: timeStamp
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
//timeStamp(15,0) = InData.winStartX.range(15,0);
|
||
|
|
//timeStamp(31,16) = InData.y.range(15,0);
|
||
|
|
OutCenteringPnt.push_back(InData);
|
||
|
|
//3rd:encInfo
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
//encInfo(15,0) = InData.winStartX.range(15,0);
|
||
|
|
//encInfo(31,16) = InData.y.range(15,0);
|
||
|
|
OutCenteringPnt.push_back(InData);
|
||
|
|
//4th: frmW, frmH
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
//frmW(15,0) = InData.winStartX.range(15,0);
|
||
|
|
//frmH(15,0) = InData.y.range(15,0);
|
||
|
|
OutCenteringPnt.push_back(InData);
|
||
|
|
//5th: frmX, WinNum
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
//frmX(15,0) = InData.winStartX.range(15,0);
|
||
|
|
WinNum = InData.LazerWinY;
|
||
|
|
OutCenteringPnt.push_back(InData);
|
||
|
|
//6th: frmY
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
//frmY(15,0) = InData.winStartX.range(15,0);
|
||
|
|
OutCenteringPnt.push_back(InData);
|
||
|
|
RgnPix zeroData = {0,0,0,0,0,0,0};
|
||
|
|
//ping-pong buffer
|
||
|
|
RgnPix HSyncData_d1 = {0,0,0,0,0,0,0};
|
||
|
|
uchar lineBuff_0[RGN_DATA_WIN_SIZE * 2];
|
||
|
|
//#pragma HLS RESOURCE variable=lineBuff_0 core=RAM_2P
|
||
|
|
uchar lineBuff_1[RGN_DATA_WIN_SIZE * 2];
|
||
|
|
//#pragma HLS RESOURCE variable=lineBuff_1 core=RAM_2P
|
||
|
|
|
||
|
|
//initialize buffer
|
||
|
|
for(int m = 0; m < RGN_DATA_WIN_SIZE * 2; m ++)
|
||
|
|
{
|
||
|
|
lineBuff_0[m] = 0;
|
||
|
|
lineBuff_1[m] = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
PeakRun linePk = {0,0,0};
|
||
|
|
uchar pkCenter = 0;
|
||
|
|
|
||
|
|
uchar lastData = 0;;
|
||
|
|
PeakRun currPeak = {0,0,0};
|
||
|
|
uchar SM = 0b00;
|
||
|
|
uchar evenFlag = 0;
|
||
|
|
uchar pkRng_S, pkRng_E;
|
||
|
|
for(int n = 0; n <= WinNum; n ++)
|
||
|
|
{
|
||
|
|
for(int i = 0; i <= 8; i ++)
|
||
|
|
{
|
||
|
|
|
||
|
|
RgnPix InData;
|
||
|
|
if(n < WinNum)
|
||
|
|
InData = InRgnPnt[readPtr++];
|
||
|
|
else
|
||
|
|
InData = zeroData;
|
||
|
|
|
||
|
|
if(0 == i) //HSync
|
||
|
|
{
|
||
|
|
if ((n > 0) &&( linePk.len>0))
|
||
|
|
OutCenteringPnt.push_back(HSyncData_d1);
|
||
|
|
HSyncData_d1 = InData;
|
||
|
|
|
||
|
|
pkRng_S = InData.RltvRdx & 0x0f;
|
||
|
|
pkRng_S += RGN_DATA_WIN_SIZE / 2;
|
||
|
|
InData.RltvRdx = InData.RltvRdx >> 4;
|
||
|
|
pkRng_E = InData.RltvRdx & 0x0f;
|
||
|
|
pkRng_E += RGN_DATA_WIN_SIZE / 2;
|
||
|
|
|
||
|
|
evenFlag = n%2;
|
||
|
|
if(0 == linePk.len)
|
||
|
|
pkCenter = RGN_DATA_WIN_SIZE/2;
|
||
|
|
else
|
||
|
|
pkCenter = linePk.start + (linePk.len >> 1);
|
||
|
|
linePk.start = 0;
|
||
|
|
linePk.len = 0;
|
||
|
|
linePk.value = 0;
|
||
|
|
lastData = 0;
|
||
|
|
currPeak.start = 0;
|
||
|
|
currPeak.len = 0;
|
||
|
|
currPeak.value = 0;
|
||
|
|
SM = 0b00; //state machine: 0b00=init state; 0b01=rising; 0b10=falling
|
||
|
|
}
|
||
|
|
else//for(int i = 0; i < 8; i ++)
|
||
|
|
{
|
||
|
|
uchar wrPos_0 = i * 2 - 2 + RGN_DATA_WIN_SIZE/2;
|
||
|
|
uchar wrPos_1 = i * 2 - 1 + RGN_DATA_WIN_SIZE/2;
|
||
|
|
uchar rdPos_0 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -2;
|
||
|
|
uchar rdPos_1 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -1;
|
||
|
|
|
||
|
|
uchar data_0 = InData.LazerWinX & 0xff;
|
||
|
|
uchar data_1 = InData.LazerWinY & 0xff;
|
||
|
|
|
||
|
|
uchar out_0, out_1;
|
||
|
|
if(0 == evenFlag)
|
||
|
|
{
|
||
|
|
lineBuff_0[wrPos_0] = data_0;
|
||
|
|
lineBuff_0[wrPos_1] = data_1;
|
||
|
|
out_0 = lineBuff_1[rdPos_0];
|
||
|
|
out_1 = lineBuff_1[rdPos_1];
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
lineBuff_1[wrPos_0] = data_0;
|
||
|
|
lineBuff_1[wrPos_1] = data_1;
|
||
|
|
out_0 = lineBuff_0[rdPos_0];
|
||
|
|
out_1 = lineBuff_0[rdPos_1];
|
||
|
|
}
|
||
|
|
//find peak
|
||
|
|
uchar preCenter = currPeak.start + (currPeak.len >> 1); //old peak center
|
||
|
|
uchar preCenter_1 = currPeak.start + ((currPeak.len+1) >> 1); //old peak center plus one
|
||
|
|
switch(SM)
|
||
|
|
{
|
||
|
|
case 0b00:
|
||
|
|
if(data_0 < data_1) //rising
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_1;
|
||
|
|
currPeak.len = 1;
|
||
|
|
currPeak.value = data_1;
|
||
|
|
SM = 0b01;
|
||
|
|
}
|
||
|
|
else if(data_0 > data_1) //falling
|
||
|
|
SM = 0b10;
|
||
|
|
break;
|
||
|
|
case 0b01: //rising
|
||
|
|
if(lastData < data_0)
|
||
|
|
{
|
||
|
|
if(data_0 < data_1)
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_1;
|
||
|
|
currPeak.len = 1;
|
||
|
|
currPeak.value = data_1;
|
||
|
|
}
|
||
|
|
else if(data_0 > data_1) //change to falling
|
||
|
|
{
|
||
|
|
if( (wrPos_0 >= pkRng_S) && (wrPos_0 <= pkRng_E) && (data_0 >linePk.value ))
|
||
|
|
{
|
||
|
|
linePk.start = wrPos_0;
|
||
|
|
linePk.len = 1;
|
||
|
|
linePk.value = data_0;
|
||
|
|
}
|
||
|
|
SM = 0b10;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_0;
|
||
|
|
currPeak.len = 2;
|
||
|
|
currPeak.value = data_0;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
else if(lastData > data_0)
|
||
|
|
{
|
||
|
|
//compare with exist
|
||
|
|
if( (preCenter >= pkRng_S) && (preCenter <= pkRng_E) && (currPeak.value > linePk.value))
|
||
|
|
{
|
||
|
|
linePk = currPeak;
|
||
|
|
}
|
||
|
|
if(data_0 < data_1)
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_1;
|
||
|
|
currPeak.len = 1;
|
||
|
|
currPeak.value = data_1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
SM = 0b10; //change to falling
|
||
|
|
}
|
||
|
|
else //lastData == data_0
|
||
|
|
{
|
||
|
|
if(data_0 < data_1)
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_1;
|
||
|
|
currPeak.len = 1;
|
||
|
|
currPeak.value = data_1;
|
||
|
|
}
|
||
|
|
else if(data_0 > data_1)
|
||
|
|
{
|
||
|
|
if( (preCenter_1 >= pkRng_S) && (preCenter_1 <= pkRng_E) && (currPeak.value > linePk.value))
|
||
|
|
{
|
||
|
|
linePk.start = currPeak.start;
|
||
|
|
linePk.value = currPeak.value;
|
||
|
|
linePk.len = currPeak.len + 1;
|
||
|
|
}
|
||
|
|
SM = 0b10; //change to falling
|
||
|
|
}
|
||
|
|
else //data_0 == dta_1
|
||
|
|
currPeak.len += 2;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 0b10: //falling
|
||
|
|
if(lastData >= data_0)
|
||
|
|
{
|
||
|
|
if(data_0 < data_1)
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_1;
|
||
|
|
currPeak.len = 1;
|
||
|
|
currPeak.value = data_1;
|
||
|
|
SM = 0b01; //rising
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else //lastData < data_0
|
||
|
|
{
|
||
|
|
if(data_0 < data_1)
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_1;
|
||
|
|
currPeak.len = 1;
|
||
|
|
currPeak.value = data_1;
|
||
|
|
SM = 0b01; //rising
|
||
|
|
}
|
||
|
|
else if(data_0 == data_1)
|
||
|
|
{
|
||
|
|
currPeak.start = wrPos_0;
|
||
|
|
currPeak.len = 2;
|
||
|
|
currPeak.value = data_0;
|
||
|
|
SM = 0b01; //rising
|
||
|
|
}
|
||
|
|
else //data_0 > data_1
|
||
|
|
{
|
||
|
|
//data_0 is a peak
|
||
|
|
if( (wrPos_0 >= pkRng_S) && (wrPos_0 <= pkRng_E) && (currPeak.value > linePk.value))
|
||
|
|
{
|
||
|
|
linePk.start = wrPos_0;
|
||
|
|
linePk.value = data_0;
|
||
|
|
linePk.len = 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
lastData = data_1; //prepare for next clock
|
||
|
|
|
||
|
|
//data output: delay one line
|
||
|
|
RgnPix stmOut = {0,0,0,0,0,0,0};
|
||
|
|
stmOut.Sync = 0b00;
|
||
|
|
stmOut.LazerWinX = out_0;
|
||
|
|
stmOut.LazerWinY = out_1;
|
||
|
|
if( (n > 0 ) && (pkCenter > 0) )
|
||
|
|
{
|
||
|
|
OutCenteringPnt.push_back(stmOut);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|