jerryzeng
dec5589bdb
-compatible to VITIS_HLS -compatible to ubuntu24.04 -change test data directory
347 lines
9.4 KiB
C++
347 lines
9.4 KiB
C++
#include "calib.h"
|
||
#include "stdlib.h"
|
||
|
||
void calib(
|
||
hls::stream<CalibData> &inCalibData,
|
||
hls::stream<RgnSubPixCalib> &outSubpixCalib
|
||
)
|
||
{
|
||
#if !(VITIS_HLS)
|
||
#pragma HLS DATA_PACK variable=inCalibData
|
||
#pragma HLS DATA_PACK variable=outSubpixCalib
|
||
#endif
|
||
#pragma HLS INTERFACE axis register both port=inCalibData
|
||
#pragma HLS INTERFACE axis register both port=outSubpixCalib
|
||
|
||
CalibData InData;
|
||
InData = inCalibData.read();
|
||
ap_uint<1> VSync = InData.Sync.range(1,1);
|
||
if(0b0 == VSync) //wait for syncFirst
|
||
return;
|
||
|
||
Byte8 dataBuff;
|
||
dataBuff.dData = InData.calibK[0];
|
||
ap_uint<12> Lines = 0;
|
||
RgnSubPixCalib outData = {0,0,0,0,0,0,0};
|
||
//1st: frameNo
|
||
outData.Sync = InData.Sync;
|
||
unsigned int data_32 = dataBuff.nData[0];
|
||
UintFloat tmp;
|
||
tmp.unData = (unsigned int)data_32;
|
||
outData.x = tmp.fdata;
|
||
outSubpixCalib.write(outData);
|
||
//2nd: timeStamp
|
||
InData = inCalibData.read();
|
||
outData.Sync = InData.Sync;
|
||
dataBuff.dData = InData.calibK[0];
|
||
data_32 = dataBuff.nData[0];
|
||
tmp.unData = (unsigned int)data_32;
|
||
outData.x = tmp.fdata;
|
||
outSubpixCalib.write(outData);
|
||
//3rd:encInfo
|
||
InData = inCalibData.read();
|
||
outData.Sync = InData.Sync;
|
||
dataBuff.dData = InData.calibK[0];
|
||
data_32 = dataBuff.nData[0];
|
||
tmp.unData = (unsigned int)data_32;
|
||
outData.x = tmp.fdata;
|
||
outSubpixCalib.write(outData);
|
||
//4th: frmW, frmH
|
||
InData = inCalibData.read();
|
||
outData.Sync = InData.Sync;
|
||
dataBuff.dData = InData.calibK[0];
|
||
data_32 = dataBuff.nData[0];
|
||
tmp.unData = (unsigned int)data_32;
|
||
outData.x = tmp.fdata;
|
||
outSubpixCalib.write(outData);
|
||
|
||
//5th: frmX, WinNum
|
||
InData = inCalibData.read();
|
||
outData.Sync = InData.Sync;
|
||
dataBuff.dData = InData.calibK[0];
|
||
data_32 = dataBuff.nData[0];
|
||
ap_uint<32> d32_t5h = data_32;
|
||
Lines(11,0) = d32_t5h.range(23,12); //lines
|
||
//outSubpixCalib.write(outData);
|
||
|
||
//6th: frmY
|
||
ap_uint<12> validDataSize = 0;
|
||
RgnSubPixCalib outData_1 = {0,0,0,0,0,0,0};
|
||
InData = inCalibData.read();
|
||
outData_1.Sync = InData.Sync;
|
||
dataBuff.dData = InData.calibK[0];
|
||
data_32 = dataBuff.nData[0];
|
||
ap_uint<32> d32_tmp = data_32;
|
||
validDataSize(11,0) = d32_tmp.range(23,12);
|
||
d32_tmp(31,12) = 0;
|
||
tmp.unData = (unsigned int)data_32;
|
||
outData_1.x = tmp.fdata;
|
||
|
||
//output validDataSize
|
||
d32_t5h(23,12) = validDataSize.range(11,0);
|
||
tmp.unData = (unsigned int)d32_t5h;
|
||
outData.x = tmp.fdata;
|
||
outSubpixCalib.write(outData);
|
||
outSubpixCalib.write(outData_1);
|
||
|
||
Byte8 dataConver_x, dataConver_y;
|
||
//double kx, ky;
|
||
float kx_1, kx_2, ky_1, ky_2;
|
||
|
||
//line_pt0
|
||
RgnSubPixCalib calibPnt;
|
||
float rawx,rawy;
|
||
double x2, x3,x4,x5,x6,x7;
|
||
double y2, y3,y4,y5,y6,y7;
|
||
float xmul0=0, xmul1=0, xmul2=0, xmul3=0, xmul4=0, xmul5=0, xmul6=0, xmul7=0;
|
||
float ymul0=0, ymul1=0, ymul2=0, ymul3=0, ymul4=0, ymul5=0, ymul6=0, ymul7=0;
|
||
|
||
//line_pt1
|
||
RgnSubPixCalib calibPnt_pt1;
|
||
float rawx_pt1,rawy_pt1;
|
||
double x2_pt1, x3_pt1,x4_pt1,x5_pt1,x6_pt1,x7_pt1;
|
||
double y2_pt1, y3_pt1,y4_pt1,y5_pt1,y6_pt1,y7_pt1;
|
||
float xmul0_pt1=0, xmul1_pt1=0, xmul2_pt1=0, xmul3_pt1=0, xmul4_pt1=0, xmul5_pt1=0, xmul6_pt1=0, xmul7_pt1=0;
|
||
float ymul0_pt1=0, ymul1_pt1=0, ymul2_pt1=0, ymul3_pt1=0, ymul4_pt1=0, ymul5_pt1=0, ymul6_pt1=0, ymul7_pt1=0;
|
||
|
||
bool vldFlag = 0;
|
||
bool vldFlag_pt1 = 0;
|
||
for(int n = 0; n < Lines; n ++)
|
||
{
|
||
#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
|
||
for(int j = 0; j <= 5; j ++) //6 clocks: 0-pt0, 1-pt1, 2<><32>3<EFBFBD><33>4<EFBFBD><34>5 - K
|
||
{
|
||
#pragma HLS PIPELINE II=1
|
||
InData = inCalibData.read();
|
||
if( 0 == j)//read subpix
|
||
{
|
||
calibPnt.x = (float)(xmul0 + xmul1 + xmul2 + xmul3 + xmul4 + xmul5+ xmul6 + xmul7);
|
||
calibPnt.y = (float)(ymul0 + ymul1 + ymul2 + ymul3 + ymul4 + ymul5+ ymul6 + ymul7);
|
||
if( (n > 0) && (vldFlag == 1))
|
||
outSubpixCalib.write(calibPnt);
|
||
|
||
dataBuff.dData = InData.calibK[0];
|
||
rawx = (float)dataBuff.fData[0];
|
||
ap_uint<32> apData32 = (ap_uint<32>)dataBuff.nData[1];
|
||
ap_uint<12> dy_12;
|
||
dy_12(11,0)= apData32.range(11, 0);
|
||
rawy = (float)dy_12;
|
||
calibPnt.rid = apData32.range(22, 12);
|
||
calibPnt.flag = apData32.range(26,23);
|
||
dataBuff.dData = InData.calibK[1];
|
||
apData32 = (ap_uint<32>)dataBuff.nData[0];
|
||
calibPnt.value = apData32.range(7,0);
|
||
calibPnt.Sync = 0;
|
||
calibPnt.rsv = 0;
|
||
if(rawx < 0)
|
||
vldFlag = 0;
|
||
else
|
||
vldFlag = 1;
|
||
|
||
double dtmp_1 = (double)rawx;
|
||
double dtmp_2 = (double)rawy;
|
||
x2 = dtmp_1 * dtmp_1;
|
||
y2 = dtmp_2 * dtmp_2;
|
||
}
|
||
else if( 1 == j)//read subpix_pt1
|
||
{
|
||
calibPnt_pt1.x = (float)(xmul0_pt1 + xmul1_pt1 + xmul2_pt1 + xmul3_pt1 + xmul4_pt1 + xmul5_pt1+ xmul6_pt1 + xmul7_pt1);
|
||
calibPnt_pt1.y = (float)(ymul0_pt1 + ymul1_pt1 + ymul2_pt1 + ymul3_pt1 + ymul4_pt1 + ymul5_pt1+ ymul6_pt1 + ymul7_pt1);
|
||
if( (n > 0) && (vldFlag_pt1 == 1))
|
||
outSubpixCalib.write(calibPnt_pt1);
|
||
|
||
dataBuff.dData = InData.calibK[0];
|
||
rawx_pt1 = (float)dataBuff.fData[0];
|
||
ap_uint<32> apData32 = (ap_uint<32>)dataBuff.nData[1];
|
||
ap_uint<12> dy_12;
|
||
dy_12(11,0)= apData32.range(11, 0);
|
||
rawy_pt1 = (float)dy_12;
|
||
calibPnt_pt1.rid = apData32.range(22, 12);
|
||
calibPnt_pt1.flag = apData32.range(26,23);
|
||
dataBuff.dData = InData.calibK[1];
|
||
apData32 = (ap_uint<32>)dataBuff.nData[0];
|
||
calibPnt_pt1.value = apData32.range(7,0);
|
||
calibPnt_pt1.Sync = 0;
|
||
calibPnt_pt1.rsv = 0;
|
||
if(rawx_pt1 < 0)
|
||
vldFlag_pt1 = 0;
|
||
else
|
||
vldFlag_pt1 = 1;
|
||
|
||
double dtmp_1 = (double)rawx_pt1;
|
||
double dtmp_2 = (double)rawy_pt1;
|
||
x2_pt1 = dtmp_1 * dtmp_1;
|
||
y2_pt1 = dtmp_2 * dtmp_2;
|
||
}
|
||
else if(2 == j)//k0, k1*x
|
||
{
|
||
//kx = InData.calibK[0];
|
||
//ky = InData.calibK[1];
|
||
dataConver_x.dData = InData.calibK[0];
|
||
kx_1 = (float)dataConver_x.fData[0];
|
||
ky_1 = (float)dataConver_x.fData[1];
|
||
dataConver_y.dData = InData.calibK[1];
|
||
kx_2 = (float)dataConver_y.fData[0];
|
||
ky_2 = (float)dataConver_y.fData[1];
|
||
|
||
xmul0 = kx_1;
|
||
ymul0 = ky_1;
|
||
xmul1 = kx_2 * rawx;
|
||
ymul1 = ky_2 * rawy;
|
||
|
||
double dtmp_1 = (double)rawx;
|
||
double dtmp_2 = (double)rawy;
|
||
x3 = x2 * dtmp_1;
|
||
y3 = y2 * dtmp_2;
|
||
|
||
xmul0_pt1 = kx_1;
|
||
ymul0_pt1 = ky_1;
|
||
xmul1_pt1 = kx_2 * rawx_pt1;
|
||
ymul1_pt1 = ky_2 * rawy_pt1;
|
||
|
||
double dtmp_3 = (double)rawx_pt1;
|
||
double dtmp_4 = (double)rawy_pt1;
|
||
x3_pt1 = x2_pt1 * dtmp_3;
|
||
y3_pt1 = y2_pt1 * dtmp_4;
|
||
}
|
||
else if(3 == j)//k2 * x^2, k3 * x^3
|
||
{
|
||
dataConver_x.dData = InData.calibK[0];
|
||
kx_1 = (float)dataConver_x.fData[0];
|
||
ky_1 = (float)dataConver_x.fData[1];
|
||
dataConver_y.dData = InData.calibK[1];
|
||
kx_2 = (float)dataConver_y.fData[0];
|
||
ky_2 = (float)dataConver_y.fData[1];
|
||
|
||
float x2_tmp = (float)x2;
|
||
float y2_tmp = (float)y2;
|
||
float x3_tmp = (float)x3;
|
||
float y3_tmp = (float)y3;
|
||
xmul2 = kx_1 * x2_tmp;
|
||
ymul2 = ky_1 * y2_tmp;
|
||
xmul3 = kx_2 * x3_tmp;
|
||
ymul3 = ky_2 * y3_tmp;
|
||
|
||
x4 = x2 * x2;
|
||
y4 = y2 * y2;
|
||
x5 = x3 * x2;
|
||
y5 = y3 * x2;
|
||
|
||
float x2_tmp_pt1 = (float)x2_pt1;
|
||
float y2_tmp_pt1 = (float)y2_pt1;
|
||
float x3_tmp_pt1 = (float)x3_pt1;
|
||
float y3_tmp_pt1 = (float)y3_pt1;
|
||
xmul2_pt1 = kx_1 * x2_tmp_pt1;
|
||
ymul2_pt1 = ky_1 * y2_tmp_pt1;
|
||
xmul3_pt1 = kx_2 * x3_tmp_pt1;
|
||
ymul3_pt1 = ky_2 * y3_tmp_pt1;
|
||
|
||
|
||
x4_pt1 = x2_pt1 * x2_pt1;
|
||
y4_pt1 = y2_pt1 * y2_pt1;
|
||
x5_pt1 = x3_pt1 * x2_pt1;
|
||
y5_pt1 = y3_pt1 * x2_pt1;
|
||
}
|
||
else if(4 == j) //k4 * x^4, k5 * x^5
|
||
{
|
||
dataConver_x.dData = InData.calibK[0];
|
||
kx_1 = (float)dataConver_x.fData[0];
|
||
ky_1 = (float)dataConver_x.fData[1];
|
||
dataConver_y.dData = InData.calibK[1];
|
||
kx_2 = (float)dataConver_y.fData[0];
|
||
ky_2 = (float)dataConver_y.fData[1];
|
||
|
||
float x4_tmp = (float)x4;
|
||
float y4_tmp = (float)y4;
|
||
float x5_tmp = (float)x5;
|
||
float y5_tmp = (float)y5;
|
||
xmul4 = kx_1 * x4_tmp;
|
||
ymul4 = ky_1 * y4_tmp;
|
||
xmul5 = kx_2 * x5_tmp;
|
||
ymul5 = ky_2 * y5_tmp;
|
||
|
||
x6 = x4 * x2;
|
||
y6 = y4 * x2;
|
||
x7 = x5 * x2;
|
||
y7 = y5 * x2;
|
||
|
||
float x4_tmp_pt1 = (float)x4_pt1;
|
||
float y4_tmp_pt1 = (float)y4_pt1;
|
||
float x5_tmp_pt1 = (float)x5_pt1;
|
||
float y5_tmp_pt1 = (float)y5_pt1;
|
||
xmul4_pt1 = kx_1 * x4_tmp_pt1;
|
||
ymul4_pt1 = ky_1 * y4_tmp_pt1;
|
||
xmul5_pt1 = kx_2 * x5_tmp_pt1;
|
||
ymul5_pt1 = ky_2 * y5_tmp_pt1;
|
||
|
||
x6_pt1 = x4_pt1 * x2_pt1;
|
||
y6_pt1 = y4_pt1 * x2_pt1;
|
||
x7_pt1 = x5_pt1 * x2_pt1;
|
||
y7_pt1 = y5_pt1 * x2_pt1;
|
||
}
|
||
else if(5 == j)//k6 * x^6, k7 * x^7
|
||
{
|
||
dataConver_x.dData = InData.calibK[0];
|
||
kx_1 = (float)dataConver_x.fData[0];
|
||
ky_1 = (float)dataConver_x.fData[1];
|
||
dataConver_y.dData = InData.calibK[1];
|
||
kx_2 = (float)dataConver_y.fData[0];
|
||
ky_2 = (float)dataConver_y.fData[1];
|
||
|
||
float x6_tmp = (float)x6;
|
||
float y6_tmp = (float)y6;
|
||
float x7_tmp = (float)x7;
|
||
float y7_tmp = (float)y7;
|
||
xmul6 = kx_1 * x6_tmp;
|
||
ymul6 = ky_1 * y6_tmp;
|
||
xmul7 = kx_2 * x7_tmp;
|
||
ymul7 = ky_2 * y7_tmp;
|
||
|
||
float x6_tmp_pt1 = (float)x6_pt1;
|
||
float y6_tmp_pt1 = (float)y6_pt1;
|
||
float x7_tmp_pt1 = (float)x7_pt1;
|
||
float y7_tmp_pt1 = (float)y7_pt1;
|
||
xmul6_pt1 = kx_1 * x6_tmp_pt1;
|
||
ymul6_pt1 = ky_1 * y6_tmp_pt1;
|
||
xmul7_pt1 = kx_2 * x7_tmp_pt1;
|
||
ymul7_pt1 = ky_2 * y7_tmp_pt1;
|
||
}
|
||
#if 0
|
||
else if(5 == j) //
|
||
{
|
||
kx = InData.calibK[0];
|
||
ky = InData.calibK[1];
|
||
|
||
x5 = x4 * rawx;
|
||
y5 = y4 * rawy;
|
||
}
|
||
else if(6 == j) //
|
||
{
|
||
kx = InData.calibK[0];
|
||
ky = InData.calibK[1];
|
||
|
||
|
||
}
|
||
else if(7 == j)//
|
||
{
|
||
kx = InData.calibK[0];
|
||
ky = InData.calibK[1];
|
||
|
||
|
||
}
|
||
else if(8 == j)//
|
||
{
|
||
kx = InData.calibK[0];
|
||
ky = InData.calibK[1];
|
||
|
||
}
|
||
#endif
|
||
}
|
||
}
|
||
if( (Lines > 0)&& (vldFlag == 1))
|
||
outSubpixCalib.write(calibPnt);
|
||
|
||
if( (Lines > 0)&& (vldFlag_pt1 == 1))
|
||
outSubpixCalib.write(calibPnt_pt1);
|
||
}
|