自动泊车与液位测量算法简介

已于 2023-06-06 16:27:28 修改
阅读量201 收藏 1
点赞数
分类专栏： 毫米波雷达 文章标签： 算法 dsp开发
于 2023-06-06 16:25:59 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_56120826/article/details/131061593
版权
本文详细对比了TI雷达平台上的两种关键算法：high_accuracy_18xx_dss（高精度液位测量）和pa_18xx_dss（自动泊车）。文章深入探讨了这两个算法的核心函数，包括高精度液位测量的累加输入和距离估计，以及自动泊车程序的1DFFT处理和数据传送。通过对这些函数的分析，展示了雷达数据处理的关键步骤和计算原理。
摘要由CSDN通过智能技术生成

TI 雷达算法解析

本文主要对比high_accuracy_18xx_dss算法和pa_18xx_dss算法

一、算法来源

high_accuracy_18xx_dss是运行在IWR1843平台的高精度液位测量程序算法；pa_18xx_dss是AWR1843雷达的自动泊车程序算法部分

二、主要函数

1.高精度液位测量程序算法部分

完成1个接收天线的多个chirp的累加

MmwDemo_processChirp(dataPathObj);

完成1DFFT变换，求1DFFT变换后的最大峰值，确认最大峰值对应距离索引，根据距离索引确认ZOOMFFT的起止细化区间；

MmwDemo_interFrameProcessing(dataPathObj)

2.自动泊车程序算法部分

完成1DFFT变换

MmwDemo_processChirp(dataPathObj, 0);

完成2DFFT、CFAR、AOA、cluster

MmwDemo_interFrameProcessing(dataPathObj, 0);

完成数据的传送

MRR_DSS_DataPathOutputLogging(dataPathObj);

三、高精度液位测量程序详细介绍

高精度液位测量程序算法部分

启动EDMA 将数据从obj->ADCdataBuf[0]搬运到obj->adcDataL2；
等待EDMA传送完成；记录当前chirp个数用于后续的数据累加；

 void MmwDemo_processChirp(MmwDemo_DSS_DataPathObj *obj)
{
	/* Kick off DMA to fetch data from ADC buffer for first channel */
     //启动DMA以从第一通道的ADC缓冲区获取数据
	EDMA_startDmaTransfer(obj->edmaHandle[EDMA_INSTANCE_A],
					   MMW_EDMA_CH_1D_IN_PING);

	//等待1D FFT数据被传输到输入缓冲器。
	MmwDemo_dataPathWait1DInputData(obj, 0);

   	obj->rangeProcInput->chirpNumber		=	obj->chirpCount;
   	obj->rangeProcInput->inputSignal		=	(cplx16_t *)&obj->ADCdataBuf[0];

   	//RUN5
	obj->rangeProcErrorCode = RADARDEMO_highAccuRangeProc_run(
			obj->haRangehandle,
			obj->rangeProcInput,
			obj->rangeProcOutput);

	obj->chirpCount++;

    if (obj->chirpCount == obj->numChirpsPerFrame)
    {
        obj->chirpCount = 0;
    }

}

3. 高精度距离运行函数RADARDEMO_highAccuRangeProc_run

RADARDEMO_highAccuRangeProc_run函数是雷达的正式算法处理函数，其中包括函数RADARDEMO_highAccuRangeProc_accumulateInput和函数RADARDEMO_highAccuRangeProc_rangeEst，如果chirpNum >= 0则执行累加输入函数RADARDEMO_highAccuRangeProc_accumulateInput，否则执行后面的函数RADARDEMO_highAccuRangeProc_rangeEst。

RADARDEMO_highAccuRangeProc_errorCode	RADARDEMO_highAccuRangeProc_run(
                            IN  void * handle,
							IN  RADARDEMO_highAccuRangeProc_input * rangeProcInput, 
							OUT RADARDEMO_highAccuRangeProc_output * rangeProcOutput)

{
	int32_t		chirpNum;
	RADARDEMO_highAccuRangeProc_handle *rangeProcInst;
	cplx16_t    * inputSignal;
	RADARDEMO_highAccuRangeProc_errorCode errorCode = RADARDEMO_HIGHACCURANGEPROC_NO_ERROR;

	inputSignal		=	rangeProcInput->inputSignal;
	rangeProcInst	=	(RADARDEMO_highAccuRangeProc_handle *) handle;

	if (inputSignal == NULL)
		errorCode	=	RADARDEMO_HIGHACCURANGEPROC_INOUTPTR_NOTCORRECT;

	if (rangeProcInst->win1D == NULL)
		errorCode	=	RADARDEMO_HIGHACCURANGEPROC_INOUTPTR_NOTCORRECT;

	if (errorCode > RADARDEMO_HIGHACCURANGEPROC_NO_ERROR)
		return(errorCode);
	
	chirpNum		=	(int32_t) rangeProcInput->chirpNumber;
	if (chirpNum >= 0)
	{ /*accumulating all the chirps and convert to float*/
#ifndef ARMVERSION
		RADARDEMO_highAccuRangeProc_accumulateInput(
                            rangeProcInst->nSamplesPerChirp,
                            rangeProcInst->fft1DSize,
                            rangeProcInst->numChirpsPerFrame,
                            rangeProcInst->win1D,
							rangeProcInst->win1DLength,
							rangeProcInput->inputSignal,
							chirpNum,
							rangeProcInst->inputSig);
#else
		RADARDEMO_highAccuRangeProc_accumulateInputGeneric(
                            rangeProcInst->nSamplesPerChirp,
                            rangeProcInst->fft1DSize,
                            rangeProcInst->numChirpsPerFrame,
                            rangeProcInst->win1D,
							rangeProcInst->win1DLength,
							rangeProcInput->inputSignal,
							chirpNum,
							rangeProcInst->inputSig);
#endif
	}
	else
	{ 
		/* range measurements*/
#ifndef ARMVERSION
		RADARDEMO_highAccuRangeProc_rangeEst(
                            rangeProcInst, 
							&(rangeProcOutput->rangeEst),
							&(rangeProcOutput->deltaPhaseEst),
							&(rangeProcOutput->linearSNREst));
#else
		if (chirpNum == -1)
		{
			RADARDEMO_highAccuRangeProc_rangeEstGeneric(
							1,
                            rangeProcInst, 
							&(rangeProcOutput->rangeEst),
							&(rangeProcOutput->deltaPhaseEst),
							&(rangeProcOutput->linearSNREst));
		}
		else
		{
			RADARDEMO_highAccuRangeProc_rangeEstGeneric(
							2,
                            rangeProcInst, 
							&(rangeProcOutput->rangeEst),
							&(rangeProcOutput->deltaPhaseEst),
							&(rangeProcOutput->linearSNREst));
		}
#endif
	}
	return(errorCode);
}

3.1 RADARDEMO_highAccuRangeProc_accumulateInput

RADARDEMO_highAccuRangeProc_accumulateInput首先累加完一根天线的所有chirp，如果累加到最后一个chirp（nChirpPerFrame - 1）则执行加窗处理。

void	RADARDEMO_highAccuRangeProc_accumulateInput(
                            IN  uint32_t nSamplesPerChirp,
                            IN  uint32_t fftSize1D,
                            IN  uint32_t nChirpPerFrame,
                            IN  float   * RESTRICT fftWin1D,
							IN 	int16_t  fftWin1DSize,
							IN  cplx16_t * RESTRICT inputPtr,
							IN  int32_t  chirpInd,
							OUT float * outputPtr)
{
	int32_t i, itemp;
	int64_t input;

	if (chirpInd == 0)
	{
		for( i = 0; i < ((int32_t)nSamplesPerChirp>> 1); i++)
		{
			input							=	_amem8(&inputPtr[2 * i]);
			_amem8_f2(&outputPtr[4*i])		=	_ftof2((float)_ext(_loll(input), 16, 16), (float)_ext(_loll(input), 0, 16));
			_amem8_f2(&outputPtr[4*i + 2])	=	_ftof2((float)_ext(_hill(input), 16, 16), (float)_ext(_hill(input), 0, 16));
		}
		if((int32_t)nSamplesPerChirp & 1)
		{
			itemp							=	_amem4(&inputPtr[nSamplesPerChirp - 1]);
			_amem8_f2(&outputPtr[2*(nSamplesPerChirp - 1)])	=	_ftof2((float)_ext(itemp, 16, 16), (float)_ext(itemp, 0, 16));
		}
	}
	else
	{
		for( i = 0; i < ((int32_t)nSamplesPerChirp>> 1); i++)
		{
			input							=	_amem8(&inputPtr[2 * i]);
			_amem8_f2(&outputPtr[4*i])		=	_daddsp(_amem8_f2(&outputPtr[4*i]), _ftof2((float)_ext(_loll(input), 16, 16), (float)_ext(_loll(input), 0, 16)));
			_amem8_f2(&outputPtr[4*i + 2])	=	_daddsp(_amem8_f2(&outputPtr[4*i + 2]), _ftof2((float)_ext(_hill(input), 16, 16), (float)_ext(_hill(input), 0, 16)));
		}
		if((int32_t)nSamplesPerChirp & 1)
		{
			itemp							=	_amem4(&inputPtr[nSamplesPerChirp - 1]);
			_amem8_f2(&outputPtr[2*(nSamplesPerChirp - 1)]) = _daddsp(_amem8_f2(&outputPtr[2*(nSamplesPerChirp - 1)]), _ftof2((float)_ext(itemp, 16, 16), (float)_ext(itemp, 0, 16)));
		}

		if (chirpInd == ((int32_t)nChirpPerFrame - 1))
		{
			__float2_t f2win1D;

			for( i = 0; i < fftWin1DSize; i++)
			{
				f2win1D						=	_ftof2(fftWin1D[i], fftWin1D[i]);///* Create a double from 2 floats  */
				_amem8_f2(&outputPtr[2*i])	=	_dmpysp(_amem8_f2(&outputPtr[2*i]), f2win1D);
				_amem8_f2(&outputPtr[2*(nSamplesPerChirp - i - 1)])	=	_dmpysp(_amem8_f2(&outputPtr[2*(nSamplesPerChirp - i - 1)]), f2win1D);
			}
			if (((int32_t)fftSize1D - (int32_t)nSamplesPerChirp) > 0)
			{
				for( i = 0; i < ((int32_t)fftSize1D - (int32_t)nSamplesPerChirp); i++);
				{
					_amem8_f2(&outputPtr[2*nSamplesPerChirp + 2*i])	=	_ftof2(0.f, 0.f);
				}
			}
		}
	}
}

3.2 RADARDEMO_highAccuRangeProc_rangeEst

RADARDEMO_highAccuRangeProc_accumulateInput首先执行FFT变换，对FFT之后的结果左右赋0，查询fft1DSize中powerPtr最大值，并得到粗略的距离仓索引coarseRangeInd，进而确定ZOOMFFT的起止索引（zoomStartInd，zoomEndInd），对该区间的进一步搜索最优峰值，然后根据距离运算公式：R=(C/2K)Fi=(cT/2B)Fi 得到比较精确地距离*estRange。

void	RADARDEMO_highAccuRangeProc_rangeEst(
                            IN  RADARDEMO_highAccuRangeProc_handle *highAccuRangeHandle,
							OUT  float * estRange,
							OUT  float * deltaPhaseEst,
							OUT float * estLinearSNR)
{
	int32_t			i, j, k, rad1D, coarseRangeInd;
	unsigned char	* brev = NULL;
	float			totalPower, max, ftemp, sigPower, * RESTRICT powerPtr;
	__float2_t		f2input, * RESTRICT inputPtr, * RESTRICT inputPtr1;

	int32_t			zoomStartInd, zoomEndInd, itemp, tempIndFine1, tempIndFine2, tempIndCoarse, indMask, shift;
	int32_t			fineRangeInd, tempFineSearchIdx, tempCoarseSearchIdx;
	__float2_t		*RESTRICT wncPtr, wncoarse, *RESTRICT wnfPtr, wnfine1, sigAcc, f2temp;
	double			freqFineEst, fdelta, interpIndx;
	float           currP, prevP, maxPrevP, maxNextP;

	j  = 30 - _norm(highAccuRangeHandle->fft1DSize);//求j=log2(fft1DSize)
	if ((j & 1) == 0)
		rad1D = 4;  //基4
	else
		rad1D = 2;  //基2

	/*由于FFT函数将损坏输入，因此需要复制到草稿。为了放大FFT，我们需要保留*/
	/* copy to scratch is needed because FFT function will corrupt the input. We need to preserve if for zoom-in FFT */
	inputPtr	=	(__float2_t *) highAccuRangeHandle->inputSig;//原始数据保留
	inputPtr1	=	(__float2_t *) &highAccuRangeHandle->scratchPad[2 * highAccuRangeHandle->fft1DSize];//2倍的fft1DSize估计是凑成64位double型
	for (i = 0; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
	{
		f2input					=	_amem8_f2(inputPtr);
		_amem8_f2(inputPtr1++)	=	f2input;//8位8位的存给暂存指针inputPtr
		_amem8_f2(inputPtr)   =	_ftof2(_lof2(f2input), _hif2(f2input));//f2input低32位加高32位组成64位double型
		inputPtr++;
	}

	inputPtr1	=	(__float2_t *) &highAccuRangeHandle->scratchPad[2 * highAccuRangeHandle->fft1DSize];
	//调用回inputPtr1的初始地址，因为上面的for循环存储数据时inputPtr1的指针地址往后增加了

	DSPF_sp_fftSPxSP (
			highAccuRangeHandle->fft1DSize, //复样本中的FFT长度
			(float*) inputPtr1,             //指向复杂数据输入的指针
			(float *)highAccuRangeHandle->twiddle, //指向复杂旋转因子的指针
			highAccuRangeHandle->fft1DOutSig,   //指向复杂输出数据的指针
			brev,                               //指向包含64个条目的位反转表的指针
			rad1D,                              //如果N可以表示为4的幂，则N_ min应为2
			0,                                  //从主fft开始的子fft复样本中的索引
			highAccuRangeHandle->fft1DSize);

	for( i = 0; i < (int32_t)highAccuRangeHandle->skipLeft;  i++ )
	{
		_amem8_f2(&highAccuRangeHandle->fft1DOutSig[2*i]) = _ftof2(0.f, 0.f);//将从左侧跳转的样本的输出信号设置为0
	}
	for( i = (int32_t)highAccuRangeHandle->fft1DSize- (int32_t)highAccuRangeHandle->skipRight; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
	{
		_amem8_f2(&highAccuRangeHandle->fft1DOutSig[2*i]) = _ftof2(0.f, 0.f);//将从右侧跳转的样本的输出信号设置为0
	}
	max			=	0.f;
	totalPower	=	0.f;
	coarseRangeInd	=	0;
	inputPtr	=	(__float2_t *)highAccuRangeHandle->fft1DOutSig;
	powerPtr	=	(float *)highAccuRangeHandle->scratchPad;
	for( i = 0; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
	{
		f2input		=	_amem8_f2(inputPtr++);
		f2input		=	_dmpysp(f2input, f2input);//高32位相乘，低32位相乘
		ftemp		=	_hif2(f2input) + _lof2(f2input);//取高32位和低32位再相加
		powerPtr[i] =	ftemp;      //功率指针
		totalPower	+=	ftemp;      //总功率
		if( max < ftemp )
		{
			max			=	ftemp;  //求最大功率
			coarseRangeInd	=	i;  //粗略距离仓
		}
	}
	
	i			=	coarseRangeInd;     //存储最大功率对应的距离的id
	sigPower	=	powerPtr[i-2] + powerPtr[i-1] + powerPtr[i] + powerPtr[i+1] + powerPtr[i+2];

	*estLinearSNR	=	divsp((float)((int32_t)highAccuRangeHandle->fft1DSize  - (int32_t)highAccuRangeHandle->skipLeft - (int32_t)highAccuRangeHandle->skipRight- 5) * sigPower, (totalPower - sigPower));
	//????????

	/* zoom in FFT: assuming always size of fft1DSize x fft1DSize */
	zoomStartInd	=	coarseRangeInd - highAccuRangeHandle->numRangeBinZoomIn;//命令里输入的是2，也就是两个单元
	//粗略的单元数-用于频率估计的放大单元数;
	//其中numRangeBinZoomIn在CLI命令(MmwDemo_CLIHighAccuCfg)通过atoi把字符串argv[1]转换为int
	zoomEndInd		=	coarseRangeInd + highAccuRangeHandle->numRangeBinZoomIn;
	//粗略的单元数+用于频率估计的放大单元数
	indMask			=	highAccuRangeHandle->fft1DSize - 1;
	shift			=	30 - _norm(highAccuRangeHandle->fft1DSize);
	inputPtr		=	(__float2_t *)highAccuRangeHandle->inputSig;
	wncPtr			=	(__float2_t *)highAccuRangeHandle->wnCoarse;
	wnfPtr			=	(__float2_t *)highAccuRangeHandle->wnFine;
	max				=	0.f;
	itemp			=	0;
	currP			=	0.f;
	prevP			=	0.f;
	maxNextP		=	0.f;
	maxPrevP		=	0.f;
	for( i = zoomStartInd; i < zoomEndInd; i++)
	{
		for( j = 0; j < (int32_t)highAccuRangeHandle->fft1DSize; j ++)
		{
			tempFineSearchIdx	=	j;//精细查询ID
			sigAcc  =	_ftof2(0.f, 0.f); 
			tempCoarseSearchIdx	=	0;//粗略查询ID
#ifdef _TMS320C6X
#pragma UNROLL(2);
#endif
			for( k = 0; k < (int32_t)highAccuRangeHandle->nSamplesPerChirp; k ++)
			{
#if 0
				tempIndFine1	=	tempFineSearchIdx & indMask;
				tempIndFine2	=	tempFineSearchIdx >> shift;
				tempFineSearchIdx		+=	j;
				tempIndCoarse	=	tempCoarseSearchIdx & indMask;
				tempCoarseSearchIdx		+=	i;

				f2input			=	_amem8_f2(&inputPtr[k]);
				wncoarse		=	_amem8_f2(&wncPtr[tempIndCoarse]);
				wnfine1			=	_amem8_f2(&wnfPtr[tempIndFine1]);
				wnfine2			=	_amem8_f2(&wncPtr[tempIndFine2]);

				f2temp			=	_complex_mpysp(f2input, wnfine1);
				f2temp			=	_complex_mpysp(f2temp, wnfine2);
				f2temp			=	_complex_mpysp(f2temp, wncoarse);
				sigAcc			=	_daddsp(sigAcc, f2temp);
#else
				//?????????????
				tempIndFine1	=	tempFineSearchIdx & indMask;
				tempIndFine2	=	tempFineSearchIdx >> shift;
				tempFineSearchIdx		+=	j;
				tempIndCoarse	=	(tempCoarseSearchIdx + tempIndFine2) & indMask;
				tempCoarseSearchIdx		+=	i;

				f2input			=	_amem8_f2(&inputPtr[k]);
				wncoarse		=	_amem8_f2(&wncPtr[tempIndCoarse]);
				wnfine1			=	_amem8_f2(&wnfPtr[tempIndFine1]);

				f2temp			=	_complex_mpysp(f2input, wnfine1);
				//两个复数相乘：(a+bi)*(c+bi)=(ac-bd)+(ad+bc)i;
				f2temp			=	_complex_mpysp(f2temp, wncoarse);
				sigAcc			=	_daddsp(sigAcc, f2temp);//两个复数相加
#endif
			}
			prevP				=	currP;
			f2temp				=	_dmpysp(sigAcc,sigAcc);//取平方和
			ftemp				=	_hif2(f2temp) + _lof2(f2temp);
			currP				=	ftemp;
			if( max < ftemp)
			{
				max				=	ftemp;
				fineRangeInd	=	itemp;//精细距离仓
				maxPrevP		=	prevP;//最大的期待功率
			}
			if(itemp == fineRangeInd + 1)
				maxNextP		=	currP;
			itemp++;
		}
	}

	interpIndx  =   0.5 * divdp((double)maxPrevP - (double)maxNextP, (double)maxPrevP + (double)maxNextP -2.0 * (double) max);

	//case1:最大拍频(maxBeatFreq)在TI里设置跟ADC采样率一致，查了查资料，
    //对于real-baseband，采样率Fs>2*中频频率，对于complex-baseband，采样率Fs>中频频率,
    //TI的数据默认是复数,也就是一个数由两部分组成
	fdelta			=	divdp((double)highAccuRangeHandle->maxBeatFreq, (double)highAccuRangeHandle->fft1DSize * (double)highAccuRangeHandle->fft1DSize);
	freqFineEst		=	fdelta * ((double)(zoomStartInd * highAccuRangeHandle->fft1DSize + fineRangeInd) + interpIndx); 

	*estRange		=	(float)divdp(freqFineEst * 3.0e8 * (double)highAccuRangeHandle->chirpRampTime, (2.0 * (double)highAccuRangeHandle->chirpBandwidth));
	//距离运算公式：R=(C/2K)Fi=(cT/2B)Fi
	if (highAccuRangeHandle->enablePhaseEst)    //启用使用相位校正的估计
	{
		float phaseCoarseEst1, phaseCoarseEst2, phaseInitial;
		double PI = 3.14159265358979323846f;
		double real, imag, denom, initReal, initImag, dtemp1, dtemp2, dtemp;
		float phaseEst, totalPhase = 0.f, phaseCorrection, rangePhaseCorrection;
		__float2_t demodSig, corrSig;

		inputPtr			=	(__float2_t *)highAccuRangeHandle->inputSig;
		phaseCoarseEst1		=	2 * (float)PI * highAccuRangeHandle->fc * (divsp(2 * (*estRange), (float)3e8) + highAccuRangeHandle->adcStartTimeConst);
		phaseCoarseEst2		=	(float)PI * highAccuRangeHandle->chirpSlope * (divsp(2 * (*estRange), (float)3e8)  + highAccuRangeHandle->adcStartTimeConst) * (divsp(2 * (*estRange), (float)3e8)  + highAccuRangeHandle->adcStartTimeConst);
		phaseInitial		=	phaseCoarseEst1 - phaseCoarseEst2;

		denom				=	divdp(1.0, (double)highAccuRangeHandle->fft1DSize);
#if 0
		dtemp1				=	cos((double)phaseInitial);
		dtemp2				=	sin(-(double)phaseInitial);
		initReal			=	cos(2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom) ;
		initImag			=	sin(-2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom);
#else
		dtemp1				=	cosdp_i((double)phaseInitial);
		dtemp2				=	sindp_i(-(double)phaseInitial);
		initReal			=	cosdp_i(2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom) ;
		initImag			=	sindp_i(-2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom);
#endif
		
		//sample @ t = 0;
		corrSig				=	_ftof2((float)dtemp1, (float)dtemp2);
		demodSig			=	_complex_mpysp(_amem8_f2(inputPtr++), corrSig);
		RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
		totalPhase			+=	phaseEst;

		if ((highAccuRangeHandle->enableFilter == 0) && (highAccuRangeHandle->enableLinearFit == 0))
		{
			//sample @ t = 1;
			dtemp			=	dtemp1 * initReal - dtemp2 * initImag;
			imag			=	dtemp1 * initImag + dtemp2 * initReal;
			real			=	dtemp;
			corrSig				=	_ftof2((float)real, (float)imag);
			demodSig			=	_complex_mpysp(_amem8_f2(inputPtr++), corrSig);
			RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
			totalPhase			+=	phaseEst;

			for( j = 2; j < (int32_t)highAccuRangeHandle->fft1DSize; j ++)
			{
				dtemp			=	real * initReal - imag * initImag;
				imag			=	real * initImag + imag * initReal;
				real			=	dtemp;
				corrSig			=	_ftof2((float)real, (float)imag);
				demodSig		=	_complex_mpysp(_amem8_f2(inputPtr++), corrSig);
				RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
				totalPhase		+=	phaseEst;
			}
			phaseCorrection		=	totalPhase * (float)denom;
			rangePhaseCorrection= divsp((phaseCorrection*(float)3e8), (4.f * (float) PI * highAccuRangeHandle->fc));
			*estRange			+=	rangePhaseCorrection;
		}
		else
		{
			//Not implemented yet
		}
	}
	return;
}

四、自动泊车程序详细介绍

4.1 MmwDemo_processChirp(MmwDemo_DSS_DataPathObj *obj, uint8_t subframeIndx)

类似于3中提到的MmwDemo_processChirp(dataPathObj)，该程序使能了3根发射天线4根接收天线，组成12根虚拟天线。每当用于1DFFT功能的函数MmwDemo_interChirpProcessing执行完一次，obj->chirpCount++，obj->txAntennaCount++，obj->dopplerBinCount++，直到obj->txAntennaCount == obj->numTxAntennas 并且 obj->dopplerBinCount == obj->numDopplerBins时 obj->chirpCount = 0;其中numDopplerBins=64，numTxAntennas=3，所以完成的chirp总数为3*64。

void MmwDemo_processChirp(MmwDemo_DSS_DataPathObj *obj, uint8_t subframeIndx)
{
    volatile uint32_t startTime;
    uint32_t radarCubeOffset;
    uint8_t chId;

    /** 1. Book keeping. */
    startTime = Cycleprofiler_getTimeStamp();

    if (obj->chirpCount > 1) //verify if ping(or pong) buffer is free for odd(or even) chirps
    {
        MmwDemo_dataPathWait1DOutputData(obj, pingPongId(obj->chirpCount), subframeIndx);
    }
    gCycleLog.interChirpWaitTime += Cycleprofiler_getTimeStamp() - startTime;

    /** 2.  Range processing. */
    MmwDemo_interChirpProcessing(obj, pingPongId(obj->chirpCount), subframeIndx);

    /* Modify destination address in Param set and DMA for sending 1DFFT output (for all antennas) to L3  */
    /*淇敼鍙傛暟闆嗗拰DMA涓殑鐩爣鍦板潃锛屼互灏�1DFFT杈撳嚭锛堥�傜敤浜庢墍鏈夊ぉ绾匡級鍙戦�佸埌L3 */
    if (isPong(obj->chirpCount))
    {
        /* select the appropriate channel based on the index of the subframe. */
        if (subframeIndx == 0)
        {
            chId = MRR_SF0_EDMA_CH_1D_OUT_PONG;
        }
        else
        {
            chId = MRR_SF1_EDMA_CH_1D_OUT_PONG;
        }

        radarCubeOffset = (obj->numDopplerBins * obj->numRxAntennas * (obj->txAntennaCount))
                            + obj->dopplerBinCount
                            + (obj->numDopplerBins * obj->numRxAntennas * obj->numTxAntennas * obj->chirpTypeCount);
        EDMAutil_triggerType3(
            obj->edmaHandle[EDMA_INSTANCE_DSS],
            (uint8_t *)NULL,
            (uint8_t *)(&obj->radarCube[radarCubeOffset]),
            (uint8_t)chId,
            (uint8_t)MRR_EDMA_TRIGGER_ENABLE);
    }
    else
    {
        if (subframeIndx == 0)
        {
            chId = MRR_SF0_EDMA_CH_1D_OUT_PING;
        }
        else
        {
            chId = MRR_SF1_EDMA_CH_1D_OUT_PING;
        }
        radarCubeOffset = (obj->numDopplerBins * obj->numRxAntennas * (obj->txAntennaCount))
                            + obj->dopplerBinCount
                            + (obj->numDopplerBins * obj->numRxAntennas * obj->numTxAntennas * obj->chirpTypeCount);

        EDMAutil_triggerType3(
            obj->edmaHandle[EDMA_INSTANCE_DSS],
            (uint8_t *)NULL,
            (uint8_t *)(&obj->radarCube[radarCubeOffset]),
            (uint8_t)chId,
            (uint8_t)MRR_EDMA_TRIGGER_ENABLE);
    }

    //logRadarOffset[idx] = radarCubeOffset;
    //
    //logChirpcnt[idx] = obj->chirpCount;
    //idx++;


    obj->chirpCount++;
    obj->txAntennaCount++;
    if (obj->txAntennaCount == obj->numTxAntennas)
    {
        obj->txAntennaCount = 0;
        obj->dopplerBinCount++;
        if (obj->dopplerBinCount == obj->numDopplerBins)
        {
            if (obj->processingPath == MAX_VEL_ENH_PROCESSING)
            {
                obj->chirpTypeCount++;
                obj->dopplerBinCount = 0;
                if (obj->chirpTypeCount == SUBFRAME_MRR_NUM_CHIRPTYPES)
                {
                    obj->chirpTypeCount = 0;
                    obj->chirpCount = 0;
                }
            }
            else
            {
               obj->chirpTypeCount = 0;
               obj->dopplerBinCount = 0;
               obj->chirpCount = 0;
            }
        }
    }
}

4.1.2MmwDemo_interChirpProcessing(MmwDemo_DSS_DataPathObj *obj, uint32_t chirpPingPongId, uint8_t subframeIndx)

该程序主要是实现1DFFT功能，按照四个接收天线的顺序依次将obj->adcDataIn中的数据搬运出来，加窗，1DFFT

void MmwDemo_interChirpProcessing(MmwDemo_DSS_DataPathObj *obj, uint32_t chirpPingPongId, uint8_t subframeIndx)
{
    uint32_t antIndx, waitingTime;
    volatile uint32_t startTime;
    volatile uint32_t startTime1;

    waitingTime = 0;
    startTime = Cycleprofiler_getTimeStamp();

    /* Kick off DMA to fetch data from ADC buffer for first channel */
    /*鍚姩 DMA 浠ヤ粠绗竴涓�氶亾鐨� ADC 缂撳啿鍣ㄤ腑鑾峰彇鏁版嵁*/
    MmwDemo_startDmaTransfer(obj->edmaHandle[EDMA_INSTANCE_DSS],
        MRR_SF0_EDMA_CH_1D_IN_PING,
        MRR_SF1_EDMA_CH_1D_IN_PING,
        subframeIndx);

    /* 1d fft for first antenna, followed by kicking off the DMA of fft output */
    /* 绗竴涓ぉ绾跨殑 1d fft锛岀劧鍚庡惎鍔� fft 杈撳嚭鐨� DMA */
    for (antIndx = 0; antIndx < obj->numRxAntennas; antIndx++)
    {
        /* kick off DMA to fetch data for next antenna */
        /* 鍚姩 DMA 浠ヨ幏鍙栦笅涓�涓ぉ绾跨殑鏁版嵁 */
        if (antIndx < (obj->numRxAntennas - 1))
        {
            if (isPong(antIndx))
            {
                MmwDemo_startDmaTransfer(obj->edmaHandle[EDMA_INSTANCE_DSS],
                    MRR_SF0_EDMA_CH_1D_IN_PING,
                    MRR_SF1_EDMA_CH_1D_IN_PING,
                    subframeIndx);
            }
            else
            {
                MmwDemo_startDmaTransfer(obj->edmaHandle[EDMA_INSTANCE_DSS],
                    MRR_SF0_EDMA_CH_1D_IN_PONG,
                    MRR_SF1_EDMA_CH_1D_IN_PONG,
                    subframeIndx);
            }
        }

        /* verify if DMA has completed for current antenna */
        startTime1 = Cycleprofiler_getTimeStamp();
        MmwDemo_dataPathWait1DInputData(obj, pingPongId(antIndx), subframeIndx);
        waitingTime += Cycleprofiler_getTimeStamp() - startTime1;

        //绐楀嚱鏁�
        mmwavelib_windowing16x16(
            (int16_t *)&obj->adcDataIn[pingPongId(antIndx) * obj->numRangeBins],
            (int16_t *)obj->window1D,
            obj->numAdcSamples);
        memset((void *)&obj->adcDataIn[pingPongId(antIndx) * obj->numRangeBins + obj->numAdcSamples],
            0, (obj->numRangeBins - obj->numAdcSamples) * sizeof(cmplx16ReIm_t));


        DSP_fft16x16_imre(
            (int16_t *)obj->twiddle16x16_1D,
            obj->numRangeBins,
            (int16_t *)&obj->adcDataIn[pingPongId(antIndx) * obj->numRangeBins],
            (int16_t *)&obj->fftOut1D[chirpPingPongId * (obj->numRxAntennas * obj->numRangeBins) +
            (obj->numRangeBins * antIndx)]);
    }

    gCycleLog.interChirpProcessingTime += Cycleprofiler_getTimeStamp() - startTime - waitingTime;
    gCycleLog.interChirpWaitTime += waitingTime;
}

4.2 MmwDemo_interFrameProcessing(MmwDemo_DSS_DataPathObj *obj, uint8_t subframeIndx)

当chirpCount == 0时，执行完所有1DFFT变化，进入MmwDemo_interFrameProcessing执行环节。该部分的第一个FOR循环是调用多普勒运行函数执行2DFFT（速度维度），并将执行结果存储到obj->radarProcConfig.heatMapMem用于CFAR检测，该FOR循环的循环嵌套依次是距离维度采样点数、radarDopplerProcessRun里面的12根虚拟天线processInst->nRxAnt，将任意一个距离维度的多根天线的的多普勒累计求和增加提高信噪比；radarFrameProcessRun主要是CFAR、AOA估计、聚类算法DBscan；populateOutputs是将径向距离、方位角、俯仰角转换为三维坐标（x,y,z）等参数。

void MmwDemo_interFrameProcessing(MmwDemo_DSS_DataPathObj *obj, uint8_t subframeIndx)
{
    uint32_t rangeIdx;
    volatile uint32_t startTime;
    volatile uint32_t startTime1;
    volatile uint32_t startTime2;
    volatile uint32_t startTimePoll, startTimePoll2;

//    startTime = Cycleprofiler_getTimeStamp();

    for (rangeIdx = 0; rangeIdx < obj->numRangeBins; rangeIdx++)
    {
        copyBlock((uint32_t *)&obj->radarCube[rangeIdx * obj->numVirtualAntennas * obj->numDopplerBins], (uint32_t *)&obj->dstPingPong[0], obj->numDopplerBins * obj->numVirtualAntennas);

        radarDopplerProcessRun(obj->radarProcessHandle, (cplx16_t *) &obj->dstPingPong[0], (float *) obj->dopplerProcOut[PING]);

        copyTranspose((uint32_t *)obj->dopplerProcOut[PING], (uint32_t *)&(obj->radarProcConfig.heatMapMem[rangeIdx]), obj->numDopplerBins, 0, obj->numRangeBins, 1);
    }
//    obj->cycleLog.dopplerProcCycles = Cycleprofiler_getTimeStamp() - startTime;

    radarFrameProcessRun(obj->radarProcessHandle, (void *) obj->outBuffCntxt);
//    obj->cycleLog.cfarProcCycles = obj->radarProcConfig.benchmarkPtr->buffer[obj->radarProcConfig.benchmarkPtr->bufferIdx].cfarDetectionCycles;
//    obj->cycleLog.doaProcCycles = obj->radarProcConfig.benchmarkPtr->buffer[obj->radarProcConfig.benchmarkPtr->bufferIdx].aoaCycles;

    populateOutputs(obj);
    //gCycleLog.interFrameProcessingTime += Cycleprofiler_getTimeStamp() - startTime - waitingTime;
    //gCycleLog.interFrameWaitTime += waitingTime;
}

总结

起风了1999
关注
专栏目录
超声波传感器与应用电路.zip
01-10
超声波传感器是一种广泛应用于各种领域，如机器人导航、工业自动化、距离测量、液位检测等的设备。这种传感器利用超声波的发射和接收来探测物体或计算距离。超声波是频率超过人类听觉范围（大约20kHz以上）的声波，...
（16）燃油流量和液位传感器
热门推荐
无人机开发（开源Ardupilot）
06-28 1万+
Ardupilot官网关于《Peripheral Hardware》的翻译 —《(16) 燃油流量和液位传感器》。
液位系统
weixin_34090643的博客
02-02 752
  液阻 R=dH/dQ 这里的H实际上应该是左右容器的液位差。但上图中右侧没有容器，液位差H就是容器内溶液高度。进水阀是为了保持液位稳态而设置。稳态条件下，进水阀和出水阀流量Q相同；非稳态条件下，上式中的Q是出水阀流量（所以不管稳态非稳态，上式中的Q都是出水阀流量）： 液阻描述的是液体流出容器时受到的“阻力”，所以液阻只和流出侧的Q有关，和注水测的Q无关。 液阻的定义和电阻的定义是类似...
AI图形算法之一：液位计识别
07-09 2846
AI人工智能的主要应用之一就是图形化处理和识别，之前写了两篇，分别是: AI图形算法的应用之一：通过图片模板对比发现油田漏油 AI图形算法的应用之一:仪表识别 经过几个晚上的辛苦，液位计识别也测试成功了。相比指针型仪表稍微有所不同。直接上效果 一、原图 二、模型，直接可以从原图中抓取。 三、模型匹配，并获取读数 技术相关： opencv,python,threshold二值化获取轮廓，透视矫正等 欢迎朋友们咨询 ...
超声波传感器与应用电路资料.7z
04-15
1. 自动泊车系统：超声波传感器可以帮助车辆判断与障碍物的距离，确保安全停车。 2. 安全监控：在智能家居或工厂环境中，超声波传感器可以检测入侵者或移动物体。 3. 液位检测：在水箱、油罐等容器中，超声波传感器...
超声波传感器与应用电路-综合文档
05-23
5. 环境监测：测量液位、流量，或者空气质量监控中的颗粒物检测。 6. 智能家居：例如智能马桶盖的自动开启，或者智能垃圾桶的满载检测。 在"超声波传感器与应用电路"的文档中，可能会涵盖以下内容： - 超声波传感器...
超声波传感器原理图、模块资料
05-24
超声波传感器是一种广泛应用在各种领域，如机器人导航、工业自动化、距离测量和液位监控等的设备。其工作原理基于声波的发射与接收，尤其是超声波频率高于人耳可听范围（大约20kHz以上）的声波。在本文中，我们将...
(完整word版)基于单片机超声波测距系统的设计和实现(开题报告).docx
06-23
这种特性使得超声波测距技术广泛应用于距离、厚度、液位的测量，以及汽车的自动泊车和倒车辅助系统。 设计思路主要包括两个阶段：首先，根据收集的单片机资料，对系统进行初步分析，选择合适的单片机型号，构建测量...
＜使用生成式AI对四种冒泡排序实现形式分析解释的探讨整理＞
m0_57859086的博客
10-01 1149
/冒泡排序实现1——不常用（这个版本两层for循环分别是i++、j++）// 声明三个整型变量i、j和temp用于循环控制和交换元素for (i = 0;i < n - 1;i++) { // 外层循环，从数组的第一个元素开始遍历到倒数第二个元素j < n;j++) { // 内层循环，从当前外层循环位置的下一个元素开始遍历到最后一个元素if (A[i] > A[j]) { // 如果当前元素大于下一个元素// 将当前元素存储到临时变量temp中// 将下一个元素赋值给当前元素。
【理论科学与实践技术】数学与经济管理中的学科与实用算法
wnm23的专栏
10-02 759
在现代商业环境中，数学与经济管理的结合为企业提供了强大的决策支持。本文将介绍几个主要学科，包括数学基础、经济学模型、管理学及风险管理，并探讨相关的实用算法和这些算法在中国及全球知名企业中的实际应用。
算法：按既定顺序创建目标数组
AWSDN的博客
09-29 255
本文主要介绍按既定顺序创建目标数组问题
DAY17||654.最大二叉树 |617.合并二叉树 |700.二叉搜索树中的搜索 |
2301_79865280的博客
09-29 1320
给定一个不含重复元素的整数数组。通过给定的数组构建最大二叉树，并且输出这个树的根节点。示例 ：构造二叉树一定要，因为要先找到中，才能分割左右子树。本题图解思路：本题做过上一题构建数组确实变得容易很多。代码优化方向有，不用每次找左右子树的时候构建新的数组的话效率就会高很多。（即直接用下标索引直接在原数组上操作）关于加不加if的问题，如果允许空节点进入递归，就不加。
【韩顺平Java笔记】第6章：数组、排序和查找
随手写写
10-02 728
排序是将多个数据，依指定的顺序进行排列的过程。
【算法】---归并排序（递归非递归实现）
何春秋的博客
10-03 1085
归并排序 in Java
C - Separated Lunch
最新发布
m0_74304371的博客
10-05 219
C - Separated Lunch(KEYENCE Programming Contest 2024 (AtCoder Beginner Contest 374))
代码随想录 101. 孤岛的总面积
qq_53524614的博客
10-05 138
dfs版本，在之前求总面积的基础上增加了判断是否有有边缘位置元素的部分，所以增加了一个notEdge变量，在遇到边缘元素时标记为0，以使所有输出均变为0，此外需要注意，即使遇到了边缘元素也需要先将所有陆地遍历并在visit上标记。毕竟做人需要有始有终）bfs的版本，bfs不需要做递归回溯，所以函数的变量还是不变，只需要在内部增加一个标记是否有边缘元素的变量即可。
代码随想录打卡Day46
weixin_52151595的博客
09-28 472
今天是动态规划的最后一天，终于要结束折磨了！！！今天的两道题，第一道看视频做的，第二道自己AC的，开心！！！！
代码随想录算法训练营day46
qq_56445436的博客
10-01 264
【代码】代码随想录算法训练营day46。
35.搜索插入位置
m0_59860403的博客
10-01 212
给定一个排序数组和一个目标值，在数组中找到目标值，并返回其索引。如果目标值不存在于数组中，返回它将会被按顺序插入的位置。请必须使用时间复杂度为O(log n)的算法。214。
模糊控制优化下的自动泊车算法研究与仿真
本文主要探讨了"基于模糊控制的自动泊车研究"这一主题，针对当前自动泊车技术中模糊控制算法的应用潜力，研究者提出了融合模糊控制策略与精确运动学模型的解决方案。首先，构建了一个精简的模糊规则库，这个规则库是...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值