409 lines
9.9 KiB
C++
409 lines
9.9 KiB
C++
#include "peakCentering.h"
|
|
#include "stdlib.h"
|
|
|
|
void peakCentering(
|
|
hls::stream<RgnPix> &InRgnPnt,
|
|
hls::stream<RgnPix> &OutCenteringPnt
|
|
)
|
|
{
|
|
#if !(VITIS_HLS)
|
|
#pragma HLS DATA_PACK variable=OutCenteringPnt
|
|
#pragma HLS DATA_PACK variable=InRgnPnt
|
|
#endif
|
|
#pragma HLS INTERFACE axis register both port=OutCenteringPnt
|
|
#pragma HLS INTERFACE axis register both port=InRgnPnt
|
|
|
|
//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;
|
|
ap_uint<12> WinNum = 0;
|
|
RgnPix InData;
|
|
InData = InRgnPnt.read();
|
|
if(0b10 != InData.Sync) //wait for syncFirst
|
|
return;
|
|
|
|
//1st: frameNo
|
|
//frmNo(15,0) = InData.winStartX.range(15,0);
|
|
//frmNo(31,16) = InData.y.range(15,0);
|
|
OutCenteringPnt.write(InData);
|
|
//2nd: timeStamp
|
|
InData = InRgnPnt.read();
|
|
//timeStamp(15,0) = InData.winStartX.range(15,0);
|
|
//timeStamp(31,16) = InData.y.range(15,0);
|
|
OutCenteringPnt.write(InData);
|
|
//3rd:encInfo
|
|
InData = InRgnPnt.read();
|
|
//encInfo(15,0) = InData.winStartX.range(15,0);
|
|
//encInfo(31,16) = InData.y.range(15,0);
|
|
OutCenteringPnt.write(InData);
|
|
//4th: frmW, frmH
|
|
InData = InRgnPnt.read();
|
|
//frmW(15,0) = InData.winStartX.range(15,0);
|
|
//frmH(15,0) = InData.y.range(15,0);
|
|
OutCenteringPnt.write(InData);
|
|
//5th: frmX, WinNum
|
|
InData = InRgnPnt.read();
|
|
//frmX(15,0) = InData.winStartX.range(15,0);
|
|
WinNum(11,0) = InData.LazerWinY.range(11,0);
|
|
OutCenteringPnt.write(InData);
|
|
//6th: frmY
|
|
InData = InRgnPnt.read();
|
|
//frmY(15,0) = InData.winStartX.range(15,0);
|
|
OutCenteringPnt.write(InData);
|
|
RgnPix zeroData = {0,0,0,0,0,0,0};
|
|
//ping-pong buffer
|
|
RgnPix HSyncData_d1 = {0,0,0,0,0,0,0};
|
|
ap_uint<12> LazerWinX_d1 = 0;
|
|
ap_uint<8> lineBuff_0[RGN_DATA_WIN_SIZE * 2];
|
|
#if VITIS_HLS
|
|
#pragma HLS BIND_STORAGE variable=lineBuff_0 type=RAM_2p
|
|
#else
|
|
//#pragma HLS RESOURCE variable=lineBuff_0 core=RAM_2P
|
|
#endif
|
|
|
|
ap_uint<8> lineBuff_1[RGN_DATA_WIN_SIZE * 2];
|
|
#if VITIS_HLS
|
|
#pragma HLS BIND_STORAGE variable=lineBuff_0 type=RAM_2p
|
|
#else
|
|
//#pragma HLS RESOURCE variable=lineBuff_1 core=RAM_2P
|
|
#endif
|
|
|
|
//initialize buffer
|
|
for(int m = 0; m < RGN_DATA_WIN_SIZE * 2; m ++)
|
|
{
|
|
#pragma HLS UNROLL
|
|
lineBuff_0[m] = 0;
|
|
lineBuff_1[m] = 0;
|
|
}
|
|
|
|
PeakRun linePk = {0,0,0};
|
|
ap_uint<8> pkCenter = 0;
|
|
|
|
ap_uint<8> lastData = 0;;
|
|
PeakRun currPeak = {0,0,0};
|
|
ap_uint<2> SM = 0b00;
|
|
ap_uint<1> evenFlag = 0;
|
|
ap_uint<5> pkRng_S, pkRng_E;
|
|
ap_uint<1> outFlag = 0;
|
|
ap_uint<1> lastSync = 0;
|
|
for(int n = 0; n <= WinNum; n ++)
|
|
{
|
|
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
|
for(int i = 0; i <= 8; i ++)
|
|
{
|
|
#pragma HLS PIPELINE II=1
|
|
|
|
RgnPix InData;
|
|
if(0 == lastSync)
|
|
InData = InRgnPnt.read();
|
|
else
|
|
InData = zeroData;
|
|
|
|
if(InData.Sync == 0b11)
|
|
lastSync = 1;
|
|
|
|
if(InData.LazerWinY == 510)
|
|
int kkk = 1; //debug point
|
|
|
|
if(0 == i) //HSync
|
|
{
|
|
evenFlag = n%2;
|
|
|
|
if(0 == linePk.len)
|
|
{
|
|
if(linePk.value >= 240)
|
|
pkCenter = RGN_DATA_WIN_SIZE;
|
|
else
|
|
pkCenter = 0;
|
|
}
|
|
else
|
|
pkCenter = linePk.start + (linePk.len >> 1);
|
|
HSyncData_d1.LazerWinX = LazerWinX_d1 + pkCenter;
|
|
|
|
if ((n > 0) && ( pkCenter > 0))
|
|
{
|
|
OutCenteringPnt.write(HSyncData_d1);
|
|
outFlag = 1;
|
|
}
|
|
else
|
|
outFlag = 0;
|
|
|
|
HSyncData_d1 = InData;
|
|
LazerWinX_d1 = InData.LazerWinX - RGN_DATA_WIN_SIZE;
|
|
|
|
pkRng_S = InData.RltvRdx.range(3,0) + RGN_DATA_WIN_SIZE/2;
|
|
pkRng_E = InData.RltvRdx.range(7,4) + RGN_DATA_WIN_SIZE/2;
|
|
|
|
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 ++)
|
|
{
|
|
ap_uint<8> wrPos_0 = i * 2 - 2 + RGN_DATA_WIN_SIZE/2;
|
|
ap_uint<8> lastPos = wrPos_0 -1;
|
|
ap_uint<8> wrPos_1 = i * 2 - 1 + RGN_DATA_WIN_SIZE/2;
|
|
ap_uint<8> rdPos_0 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -2;
|
|
ap_uint<8> rdPos_1 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -1;
|
|
|
|
ap_uint<8> data_0 = InData.LazerWinX.range(7,0);
|
|
ap_uint<8> data_1 = InData.LazerWinY.range(7,0);
|
|
|
|
ap_uint<8> 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
|
|
ap_uint<8> preCenter = currPeak.start + (currPeak.len >> 1); //old peak center
|
|
ap_uint<8> 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;
|
|
linePk.value = data_1; //record initial value
|
|
}
|
|
else if(data_0 > data_1) //falling
|
|
{
|
|
SM = 0b10;
|
|
currPeak.value = data_0;
|
|
linePk.value = data_0; //record initial value
|
|
}
|
|
else
|
|
{
|
|
currPeak.start = wrPos_0;
|
|
currPeak.len = 2;
|
|
currPeak.value = data_0;
|
|
SM = 0b11;
|
|
linePk.value = data_0; //record initial value
|
|
}
|
|
break;
|
|
case 0b11: //horizontal state
|
|
if(lastData < data_0) //rising
|
|
{
|
|
if(data_0 < data_1) //rising - rising. if i==8, data_1 is invalid peak,
|
|
{
|
|
currPeak.start = wrPos_1;
|
|
currPeak.len = 1;
|
|
currPeak.value = data_1;
|
|
SM = 0b01;
|
|
}
|
|
else if(data_0 > data_1) //rising - falling, data_0 is peak
|
|
{
|
|
if( (wrPos_0 >= pkRng_S) && (wrPos_0 <= pkRng_E) )
|
|
{
|
|
linePk.start = wrPos_0;
|
|
linePk.len = 1;
|
|
linePk.value = data_0;
|
|
}
|
|
SM = 0b10;
|
|
}
|
|
else //data_0 == data_1, if i==8, data_0 and data_1 is invalid peak,
|
|
{
|
|
currPeak.start = wrPos_0;
|
|
currPeak.len = 2;
|
|
currPeak.value = data_0;
|
|
SM = 0b01;
|
|
}
|
|
}
|
|
else if(lastData > data_0) //falling
|
|
{
|
|
if((currPeak.start >= pkRng_S) && (lastPos <= pkRng_E))
|
|
linePk = currPeak;
|
|
|
|
if(data_0 < data_1) //falling - rising, peak is at lastData (pos:0,1,2...), invalid
|
|
{
|
|
currPeak.start = wrPos_1;
|
|
currPeak.len = 1;
|
|
currPeak.value = data_1;
|
|
SM = 0b01;
|
|
}
|
|
else// if(data_0 >= data_1)//falling, peak is at lastData
|
|
SM = 0b10;
|
|
}
|
|
else //lastData == data_0
|
|
{
|
|
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
|
|
{
|
|
if( (currPeak.start >= pkRng_S) && (wrPos_0 >= pkRng_S))
|
|
{
|
|
linePk.start = currPeak.start;
|
|
linePk.len = currPeak.len + 1;
|
|
linePk.value = currPeak.value;
|
|
}
|
|
SM = 0b10;
|
|
}
|
|
else
|
|
currPeak.len += 2;
|
|
}
|
|
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, compare with exist
|
|
{
|
|
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( (currPeak.start >= pkRng_S) && (lastPos <= 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( (currPeak.start >= pkRng_S) && (wrPos_0 = 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;
|
|
if(i == 8) //last one
|
|
{
|
|
if( (currPeak.start >= pkRng_S) && (wrPos_1 <= pkRng_E) && (currPeak.value > linePk.value))
|
|
{
|
|
linePk = currPeak;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
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(7,0) = out_0;
|
|
stmOut.LazerWinY(7,0) = out_1;
|
|
if( outFlag == 1)
|
|
{
|
|
OutCenteringPnt.write(stmOut);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//add Last sync data
|
|
RgnPix VSync;
|
|
VSync.LazerWinFlag = 0;
|
|
VSync.LazerWinRid = 0;
|
|
VSync.LazerWinRsv = 0;
|
|
VSync.LazerWinX = 0;
|
|
VSync.LazerWinY = 0;
|
|
VSync.RltvRdx = 0;
|
|
VSync.Sync = 0b11;
|
|
OutCenteringPnt.write(VSync);
|
|
}
|