回复 6楼 kenierlee
#include "include.h"
#define NLASM
#define CAExtOutput (int)8
/*******************************************************************************
FUNCTION initCorrelator()
功能：对相关器进行初始化
*******************************************************************************/
void initCorrelator()
{
double
phase0, deltaPhase,phase;
double
amplitude, value, absValue, signValue;
long
i;
phase0 = twoPI/(2*CARRIER_TABLE_LENGTH);
deltaPhase = twoPI/CARRIER_TABLE_LENGTH;
amplitude = CARRIER_TABLE_LENGTH/2.0;
for(i=0; i<CARRIER_TABLE_LENGTH; i++)
{
phase = phase0 + deltaPhase*i;
value = sin(phase);
absValue = fabs(value);
signValue = value>=0 ? 1 : -1;
sinTable[i] = (char)(signValue*ceil(absValue*amplitude));
value = cos(phase);
absValue = fabs(value);
signValue = value>=0 ? 1 : -1;
cosTable[i] = (char)(signValue*ceil(absValue*amplitude));
}
caTable = caGen();
CAExtendedTable = caGenExtended(caTable);
genProductTable();
// 如果采用FFT进行信号捕获，则以下的初始化过程并非必要
// 因为在信号捕获到之前没有必要打开相关器对应的通道。
for (i=0;i<=chmax;i++)
{
correlator[i].fCarr = (long)(CARRIER_FREQ*deltaPhaseConst+0.5);
correlator[i].fCode = (__int64)(1.0*deltaCodePhaseConst+0.5);
correlator[i].state = CHN_OFF;
correlator[i].phaseLocal=0;
correlator[i].carrierCycle = 0;
correlator[i].ready = 1;
}
}
/*******************************************************************************
FUNCTION correlatorChannelProcess()
功能：以下函数用于仿真相关器的单个通道
*******************************************************************************/
void correlatorChannelProcess(char *data,
long dataLength,
GNSS_FPGA_CORRELATOR_STRUCT *pCorr)
{
if(pCorr->state!=CHN_ON)
return;
__int64
tau;
register long phaseLocal;
__m128i s128;
long* pls128=(long*)&s128;
long deltaPhase;
__int64
deltaCodePhase;
long counter;
char* localCaTable;
long carrierCycle;
__m128i* pChar;
long* pTau=(long *)&tau+1;
char* pData=data;
localCaTable = CAExtendedTable[pCorr->sv-1];
phaseLocal
= pCorr->phaseLocal;
// 读取相关器通道的起始载波相位
deltaPhase
= pCorr->fCarr;
// 读取载波相位的步进
carrierCycle = pCorr->carrierCycle;
// 读取载波相位的整数部分
tau = pCorr->tau;
// 读取相关器通道的起始码相位
deltaCodePhase = pCorr->fCode;
// 读取码相位的步进
*(pls128)
= (pCorr->i_early);
// 读取相关器通道的early-i支路累加值
*(pls128+1) = (pCorr->q_early);
// 读取相关器通道的early-q支路累加值
*(pls128+2) = (pCorr->i_prompt);
// 读取相关器通道的prompt-i支路累加值
*(pls128+3) = (pCorr->q_prompt);
// 读取相关器通道的prompt-q支路累加值
#ifdef NLASM
__asm
{
mov eax, pls128
movdqa
xmm5,xmmword ptr [eax]
}
#endif
for (counter=0; counter<dataLength; counter++)
{
tau += deltaCodePhase;
if (*pTau>=261888)
{
*pTau -=261888;
// 码相位超过了最大值，重新复位
// 锁存所有的I, Q积分值
#ifdef NLASM
__asm
{
mov eax, pls128
movdqa
xmmword ptr [eax],xmm5
}
#endif
pCorr->latchedSER = *(pls128);
pCorr->latchedSEI = *(pls128+1);
pCorr->latchedSPR = *(pls128+2);
pCorr->latchedSPI = *(pls128+3);
#ifdef NLASM
__asm
{
xorps
xmm5, xmm5
}
#else
*psR=*(psR+1)=*(psR+2)=0;
*psI=*(psI+1)=*(psI+2)=0;
#endif
// 产生积分中断(数据锁存)标志
pCorr->ready = 1;
}
phaseLocal += deltaPhase;
carrierCycle += (phaseLocal>>30);
phaseLocal &= 0x3FFFFFFF;
pChar = productTable[data[counter]][phaseLocal>>26][localCaTable[*pTau>>3]];
#ifdef NLASM
__asm
{
mov
eax, dword ptr [pChar]
movdqa
xmm4,xmmword ptr [eax]
psubd
xmm5,xmm4
}
#else
sI = _mm_sub_epi32 (sI,*pChar);
sR = _mm_add_epi32 (sR,*(pChar+1));
#endif
}
// end of for
#ifdef NLASM
__asm
{
mov eax, pls128
movdqa
xmmword ptr [eax],xmm5
}
#endif
// 还原数据
pCorr->i_early = *(pls128);
pCorr->q_early = *(pls128+1);
pCorr->i_prompt = *(pls128+2);
pCorr->q_prompt = *(pls128+3);
pCorr->carrierCycle = carrierCycle;
pCorr->tau = tau;
pCorr->phaseLocal = phaseLocal;
}
long lgDataLength;
char* cgData;
HANDLE chn_start_event[chmax+1];
HANDLE chn_finish_event[chmax+1];
HANDLE chn_thread[chmax+1];
int chn_id[chmax+1];
void CorrProc(LPVOID lpData)
{
int i = *(int*)lpData;
while(TRUE)
{
WaitForSingleObject(chn_start_event[i],INFINITE);
correlatorChannelProcess(cgData,lgDataLength,&correlator[i]);
SetEvent(chn_finish_event[i]);
}
}
/****************************************************************************
// 该函数仿真多个通道的相关器
// 该函数在每次发生“积分清除”或“测量”中断时被调用
// 只有处于ON状态的相关器通道才会被处理
*****************************************************************************/
void correlatorProcess(char* data, long dataLength)
{
char i;
correlatorDataLength = dataLength;
Local_time = Local_time + 0.0005;
Local_time = fmod(Local_time,604800);
if (gTotAvailCore > 1)
{
static bool bFirst=true;
cgData = data;
lgDataLength = dataLength;
if(bFirst)
{
for (i=0; i<=chmax; i++)
{
chn_id[i]=i;
chn_finish_event[i] = CreateEvent(NULL, FALSE, FALSE,NULL);
chn_start_event[i] = CreateEvent(NULL, FALSE, TRUE,NULL);
chn_thread[i] =
CreateThread((LPSECURITY_ATTRIBUTES) NULL,
0,
(LPTHREAD_START_ROUTINE)(CorrProc),
(LPVOID)&chn_id[i],
0,
NULL);
SetThreadPriority(chn_thread[i], THREAD_PRIORITY_ABOVE_NORMAL);
}
bFirst = false;
}
else
{
for (i=0; i<=chmax; i++)
SetEvent(chn_start_event[i]); // signal each correlator to start processing
}
WaitForMultipleObjects(chmax+1,chn_finish_event,TRUE,INFINITE); // wait for all channel to be ready
}
else
{
for (i=0;i<=chmax;i++)
{
correlatorChannelProcess(cgData,lgDataLength,&correlator[i]);
}
}
//----------------计算测量中断是否到来-----------------//
TIC_RT_CNTR += dataLength;
if (TIC_RT_CNTR == TIC_CNTR)
{
// TIC测量中断发生后需要锁存所有数据
// 从程序实现的角度看，这个锁存过程是不必要的
// 但是从保持数据结构的独立性上看，这个锁存是有必要的
for (i=0;i<=chmax;i++)
{
correlator[i].carrierCycleLatch = correlator[i].carrierCycle;
correlator[i].carrierCycle = 0;
correlator[i].phaseLocalLatch = correlator[i].phaseLocal;
correlator[i].tauLatch = correlator[i].tau;
}
TIC_RT_CNTR = 0;
TIC_OCCUR_FLAG = 1;
}
}
void shutCorrelator()
{
long i;
if(gTotAvailCore > 1)
{
for (i=0; i<=chmax+1; i++)
TerminateThread(chn_thread[i],0);
}
freeCaTable(caTable);
freeCAExtendedTable(CAExtendedTable);
freeProductTable();
for (i=0;i<=chmax;i++)
{
_aligned_free(buffer[i]);
}
}
char** caGenExtended(char** localCATable)
{
char **CAExtendedTable;
int i;
long deltaPhase;
long phaseP, phaseE, phaseL;
char eValue, pValue, lValue;
CAExtendedTable=(char**)malloc(sizeof(char*)*37);
if(CAExtendedTable==NULL)
{
printf("memory allocation error in CA extended table generation");
exit(0);
}
CAExtendedTable[0]=(char *)malloc(1023*CAExtTableResolution*37*sizeof(char));
if (CAExtendedTable[0]==NULL)
{
printf("memory allocation error in CA extended table generation");
exit(0);
}
for (i=1;i<37;i++)
{
CAExtendedTable[i]=CAExtendedTable[i-1]+1023*CAExtTableResolution;
}
deltaPhase = (long)(DLLdT*0.5*CAExtTableResolution+0.5);
for (i=0; i<37; i++)
{
for (phaseP=0; phaseP<1023*CAExtTableResolution; phaseP++)
{
phaseE = phaseP+deltaPhase;
if(phaseE>=1023*CAExtTableResolution)
{
phaseE-=1023*CAExtTableResolution;
}
eValue = localCATable[i][phaseE>>5];
pValue = localCATable[i][phaseP>>5];
phaseL = phaseP-deltaPhase;
if(phaseL<0)
{
phaseL+=1023*CAExtTableResolution;
}
lValue = localCATable[i][phaseL>>5];
CAExtendedTable[i][phaseP] = 0;
if(eValue>0)
{
CAExtendedTable[i][phaseP]++;
}
CAExtendedTable[i][phaseP]<<=1;
if(pValue>0)
{
CAExtendedTable[i][phaseP]++;
}
CAExtendedTable[i][phaseP]<<=1;
if(lValue>0)
{
CAExtendedTable[i][phaseP]++;
}
//CAExtendedTable[i][phaseP]<<=5;
}
}
return CAExtendedTable;
}
void freeCAExtendedTable(char** caExtendedTable)
{
free(caExtendedTable[0]);
free(caExtendedTable);
}
void genProductTable()
{
long int i,j,k,l;
char *ADOutput = new char[ADNumber];
char code;
long EPL2E_LP[8]={0,2,0,2,-2,0,-2,0};
// AD索引表
ADOutput[0] = -3;
ADOutput[1] = -1;
ADOutput[2] = 1;
ADOutput[3] = 3;
productTable = (VIRTUAL_CHAR****) malloc(sizeof(VIRTUAL_CHAR***)*ADNumber);
if(productTable==NULL)
{
perror("Product table allocation error 1");
exit(0);
}
for (i=0; i<ADNumber; i++)
{
productTable[i] = (VIRTUAL_CHAR***)malloc(sizeof(VIRTUAL_CHAR**)*CARRIER_TABLE_LENGTH);
if(productTable[i]==NULL)
{
perror("Product table allocation error 2");
exit(0);
}
for (j=0; j<CARRIER_TABLE_LENGTH; j++)
{
productTable[i][j]=(VIRTUAL_CHAR**)malloc(sizeof(VIRTUAL_CHAR*)*CAExtOutput);
if(productTable[i][j]==NULL)
{
perror("Product table allocation error 3");
exit(0);
}
for (k=0; k<CAExtOutput; k++)
{
// The 128-bit number is divided into four 32-bit parts
// E-L I, E-L Q, Prompt I, Prompt Q
productTable[i][j][k]=(VIRTUAL_CHAR*)_aligned_malloc(sizeof(VIRTUAL_CHAR),16);
if(productTable[i][j][k]==NULL)
{
perror("Product table allocation error 4");
exit(0);
}
code=EPL2E_LP[k];
long* pLong=(long*)productTable[i][j][k];
*pLong++
= ADOutput[i]*cosTable[j]*code; // E-L:I
*pLong++
=-ADOutput[i]*sinTable[j]*code; // E-L:Q
code = (k<<30)>=0 ? 1 : -1;
*pLong++
= ADOutput[i]*cosTable[j]*code; // P:I
*pLong++
=-ADOutput[i]*sinTable[j]*code; // P:Q
}
}
}
delete []ADOutput;
}
void freeProductTable()
{
int i,j,k;
for(i=0; i<ADNumber; i++)
{
for(j=0; j<CARRIER_TABLE_LENGTH; j++)
{
for(k=0; k<CAExtOutput; k++)
{
_aligned_free(productTable[i][j][k]);
}
free(productTable[i][j]);
}
free(productTable[i]);
}
free(productTable);
}
void g1Gen(char* g1)
{
short i;
for (i=0;i<10;i++)
{
g1[i]=1;
}
for (i=0;i<1023;i++)
{
g1[i+10]=g1[i+7]^g1[i];
}
}
void g2Gen(char* g2)
{
short i;
for (i=0;i<10;i++)
{
g2[i]=1;
}
for (i=0;i<1023;i++)
{
g2[i+10]=g2[i]^g2[i+1]^g2[i+2]^g2[i+4]^g2[i+7]^g2[i+8];
}
}
char** caGen(void)
{
char g1[1023+10], g2[1023+10];
short sv[37]={5,6,7,8,17,18,139,140,141,251,252,254,255,256,257,258,469,
470,471,472,473,474,509,512,513,514,515,516,859,860,861,862,863,950,947,948,950};
short i, j, index;
char* pTable;
char** LocalCATable;
LocalCATable = (char**)malloc(37*sizeof(char*));
if(LocalCATable==NULL)
{
printf("memory allocation error in CA table generation");
exit(0);
}
for (i=0;i<37;i++)
{
LocalCATable[i]=(char *)malloc(1023*sizeof(char));
if (LocalCATable[i]==NULL)
{
printf("memory allocation error in CA table generation");
exit(0);
}
}
g1Gen(g1);
g2Gen(g2);
for (i=0;i<37;i++)
{
pTable = LocalCATable[i];
for (j=0; j<1023; j++)
{
index = (j+1023-sv[i])%1023;
// 1 -> 1
0 -> -1
*pTable++ = (g2[index]^g1[j])*2-1;
}
}
return LocalCATable;
}
void freeCaTable(char** caTable)
{
short i;
for (i=0;i<37;i++)
{
free(caTable[i]);
}
free(caTable);
}
是这部分吗？
