jerryzeng/camAlgo
2
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
camAlgo/camAlgoSW/sourceCode/peakCentering.cpp

277 lines
6.8 KiB
C++
Raw Blame History

#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};
ushort LazerWinX_d1 = 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
{
evenFlag = n % 2;
if (0 == linePk.len)
pkCenter = RGN_DATA_WIN_SIZE / 2;
else
pkCenter = linePk.start + (linePk.len >> 1);
HSyncData_d1.LazerWinX = LazerWinX_d1 + pkCenter;
if (HSyncData_d1.LazerWinY == 578)
int kkk = 1;
if ((n > 0) &&( linePk.len>0))
OutCenteringPnt.push_back(HSyncData_d1);
HSyncData_d1 = InData;
LazerWinX_d1 = InData.LazerWinX - RGN_DATA_WIN_SIZE;
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;
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);
}
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink