diff --git a/Calib/sourceCode/Calib.cpp b/Calib/sourceCode/Calib.cpp index 4bf4842..59d7376 100644 --- a/Calib/sourceCode/Calib.cpp +++ b/Calib/sourceCode/Calib.cpp @@ -19,7 +19,7 @@ void calib( Byte8 dataBuff; dataBuff.dData = InData.calibK[0]; - ap_uint<12> WinNum = 0; + ap_uint<12> Lines = 0; RgnSubPixCalib outData = {0,0,0,0,0,0,0}; //1st: frameNo outData.Sync = InData.Sync; @@ -59,7 +59,7 @@ void calib( dataBuff.dData = InData.calibK[0]; data_32 = dataBuff.nData[0]; ap_uint<32> d32_t5h = data_32; - WinNum(11,0) = d32_t5h.range(23,12); + Lines(11,0) = d32_t5h.range(23,12); //lines //outSubpixCalib.write(outData); //6th: frmY @@ -82,20 +82,32 @@ void calib( 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; - double rawx,rawy; + float rawx,rawy; double x2, x3,x4,x5,x6,x7; double y2, y3,y4,y5,y6,y7; - //double kx, ky; - double kx_1, kx_2, ky_1, ky_2; - Byte8 dataConver_x, dataConver_y; - double xmul0=0, xmul1=0, xmul2=0, xmul3=0, xmul4=0, xmul5=0, xmul6=0, xmul7=0; - double ymul0=0, ymul1=0, ymul2=0, ymul3=0, ymul4=0, ymul5=0, ymul6=0, ymul7=0; + 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; - for(int n = 0; n < WinNum; n ++) + bool vldFlag_pt1 = 0; + for(int n = 0; n < Lines; n ++) { #pragma HLS LOOP_TRIPCOUNT min=4096 max=4096 - for(int j = 0; j <= 4; j ++) + for(int j = 0; j <= 5; j ++) //6 clocks: 0-pt0, 1-pt1, 2£¬3£¬4£¬5 - K { #pragma HLS PIPELINE II=1 InData = inCalibData.read(); @@ -107,11 +119,11 @@ void calib( outSubpixCalib.write(calibPnt); dataBuff.dData = InData.calibK[0]; - rawx = (double)dataBuff.fData[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 = (double)dy_12; + rawy = (float)dy_12; calibPnt.rid = apData32.range(22, 12); calibPnt.flag = apData32.range(26,23); dataBuff.dData = InData.calibK[1]; @@ -124,77 +136,173 @@ void calib( else vldFlag = 1; - x2 = rawx * rawx; - y2 = rawy * rawy; + double dtmp_1 = (double)rawx; + double dtmp_2 = (double)rawy; + x2 = dtmp_1 * dtmp_1; + y2 = dtmp_2 * dtmp_2; } - else if(1 == j)//k0, k1*x + 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 = (double)dataConver_x.fData[0]; - kx_2 = (double)dataConver_x.fData[1]; + kx_1 = (float)dataConver_x.fData[0]; + kx_2 = (float)dataConver_x.fData[1]; dataConver_y.dData = InData.calibK[0]; - ky_1 = (double)dataConver_y.fData[0]; - ky_2 = (double)dataConver_y.fData[1]; + ky_1 = (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; - x3 = x2 * rawx; - y3 = y2 * 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(2 == j)//k2 * x^2, k3 * x^3 + else if(3 == j)//k2 * x^2, k3 * x^3 { dataConver_x.dData = InData.calibK[0]; - kx_1 = (double)dataConver_x.fData[0]; - kx_2 = (double)dataConver_x.fData[1]; + kx_1 = (float)dataConver_x.fData[0]; + kx_2 = (float)dataConver_x.fData[1]; dataConver_y.dData = InData.calibK[0]; - ky_1 = (double)dataConver_y.fData[0]; - ky_2 = (double)dataConver_y.fData[1]; + ky_1 = (float)dataConver_y.fData[0]; + ky_2 = (float)dataConver_y.fData[1]; - xmul2 = kx_1 * x2; - ymul2 = ky_1 * y2; - xmul3 = kx_2 * x3; - ymul3 = ky_2 * y3; + 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 = x3 * rawx; - y4 = y3 * rawy; + 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(3 == j) //k4 * x^4, k5 * x^5 + else if(4 == j) //k4 * x^4, k5 * x^5 { dataConver_x.dData = InData.calibK[0]; - kx_1 = (double)dataConver_x.fData[0]; - kx_2 = (double)dataConver_x.fData[1]; + kx_1 = (float)dataConver_x.fData[0]; + kx_2 = (float)dataConver_x.fData[1]; dataConver_y.dData = InData.calibK[0]; - ky_1 = (double)dataConver_y.fData[0]; - ky_2 = (double)dataConver_y.fData[1]; + ky_1 = (float)dataConver_y.fData[0]; + ky_2 = (float)dataConver_y.fData[1]; - xmul4 = kx_1 * x4; - ymul4 = ky_1 * y4; - xmul5 = kx_2 * x5; - ymul5 = ky_2 * y5; + 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 = x5 * rawx; - y6 = y5 * rawy; + 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(4 == j)//k6 * x^6, k7 * x^7 + else if(5 == j)//k6 * x^6, k7 * x^7 { dataConver_x.dData = InData.calibK[0]; - kx_1 = (double)dataConver_x.fData[0]; - kx_2 = (double)dataConver_x.fData[1]; + kx_1 = (float)dataConver_x.fData[0]; + kx_2 = (float)dataConver_x.fData[1]; dataConver_y.dData = InData.calibK[0]; - ky_1 = (double)dataConver_y.fData[0]; - ky_2 = (double)dataConver_y.fData[1]; + ky_1 = (float)dataConver_y.fData[0]; + ky_2 = (float)dataConver_y.fData[1]; - xmul6 = kx_1 * x6; - ymul6 = ky_1 * y6; - xmul7 = kx_2 * x7; - ymul7 = ky_2 * y7; + 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) // @@ -228,6 +336,9 @@ void calib( #endif } } - if( (WinNum > 0)&& (vldFlag == 1)) + if( (Lines > 0)&& (vldFlag == 1)) outSubpixCalib.write(calibPnt); + + if( (Lines > 0)&& (vldFlag_pt1 == 1)) + outSubpixCalib.write(calibPnt_pt1); } diff --git a/Calib/sourceCode/calib_tb.cpp b/Calib/sourceCode/calib_tb.cpp index 86785c1..981ad43 100644 --- a/Calib/sourceCode/calib_tb.cpp +++ b/Calib/sourceCode/calib_tb.cpp @@ -89,9 +89,9 @@ void genCalibTestSream( VSync.Sync = 0b00; calibStream.write(VSync); //5th: frmX, WinNum - int winNum = (int)subpixData.size(); + int lines = (int)subpixData.size() / 2; //lines data16_1 = frameROI_x; - data16_2 = (uint16_t)winNum; + data16_2 = (uint16_t)lines; tmpData(11,0) = data16_1.range(11,0); tmpData(23,12) = data16_2.range(11,0); tmpData(31,24) = 0; @@ -112,9 +112,9 @@ void genCalibTestSream( VSync.Sync = 0b00; calibStream.write(VSync); //output data - for(int i = 0; i < winNum; i ++) + for(int i = 0; i < lines; i ++) { - RgnSubPix a_line = subpixData[i]; + RgnSubPix a_line = subpixData[i*2]; CalibData conData; dataBuff.fData[0] = a_line.x; @@ -133,6 +133,22 @@ void genCalibTestSream( conData.Sync = 0b01; calibStream.write(conData); + a_line = subpixData[i*2+1]; + dataBuff.fData[0] = a_line.x; + apData32(11, 0) = a_line.y.range(11,0); + apData32(22, 12) = a_line.rid.range(10,0); + apData32(26,23) = a_line.flag(3,0); + apData32(31,27) = 0; + dataBuff.nData[1] = (uint32_t)apData32; + conData.calibK[0] = dataBuff.dData; + apData32(7,0) = a_line.value.range(7,0); + apData32(31,8) = 0; + dataBuff.nData[0] = (uint32_t)apData32; + dataBuff.nData[1] = 0; + conData.calibK[1] = dataBuff.dData; + conData.Sync = 0b00; + calibStream.write(conData); + for(int m = 0; m < 4; m ++) //write k { conData.Sync = 0b00; diff --git a/subPix/sourceCode/subPix.cpp b/subPix/sourceCode/subPix.cpp index 49d4003..92d0080 100644 --- a/subPix/sourceCode/subPix.cpp +++ b/subPix/sourceCode/subPix.cpp @@ -3,7 +3,7 @@ void subpix( hls::stream &InConvolvePnt, - hls::stream &OutSubpixPnt, + hls::stream &OutSubpixPnt ) { #pragma HLS DATA_PACK variable=InConvolvePnt @@ -63,9 +63,9 @@ void subpix( OutSubpixPnt.write(outData); //5th: frmX, WinNum int nLines = (int)frmH; - int nLoops = nLines * 2; - if(nLoops >= 4096) - nLoops = 4095; + int nLoops = nLines; // * 2; + //if(nLoops >= 4096) + // nLoops = 4095; ap_uint<12> outSize = (ap_uint<12>)nLoops; InData = InConvolvePnt.read(); outData.Sync = InData.Sync; @@ -95,7 +95,8 @@ void subpix( InData = InConvolvePnt.read(); readNum ++; RgnPixConvolve currData; - for(int n = 0; n < nLoops; n ++) + int loopNum = nLoops * 2; + for(int n = 0; n < loopNum; n ++) { #pragma HLS LOOP_TRIPCOUNT min=4096 max=4096 #pragma HLS PIPELINE II=1 diff --git a/subPix/sourceCode/subPix_tb.cpp b/subPix/sourceCode/subPix_tb.cpp index 9214abd..85194f4 100644 --- a/subPix/sourceCode/subPix_tb.cpp +++ b/subPix/sourceCode/subPix_tb.cpp @@ -8,8 +8,7 @@ #include "..\..\globals\tbGlobals.h" std::vector readConvolveDataFile( - const char* file) -{ + const char* file){ std::ifstream inputFile(file); std::string strLineTxt; @@ -174,7 +173,7 @@ void convertSubpixStreamToArray( tmp.fdata = VSync.x; *frameROI_y = tmp.unData & 0xfff; //output data - for(int i = 0; i < winNum; i ++) + for(int i = 0; i < winNum*2; i ++) { RgnSubPix data_in = streamData.read(); Luma_rgnSubpix a_pnt;