So the function AoAProcDSP_XYZestimation performs the translation from range azimuth and elevation into XYZ as we see:
static uint32_t AoAProcDSP_XYZestimation
(
AOADspObj *aoaDspObj,
uint32_t objOutIdx,
uint32_t objInCfarIdx,
uint32_t maxIdx,
int32_t dopplerSignIdx
)
{
int32_t sMaxIdx;
float temp;
float Wx, Wz;
float range;
float limitScale;
float x, y, z;
float peakAzimRe, peakAzimIm, peakElevRe, peakElevIm;
DPU_AoAProcDSP_HW_Resources *res = &aoaDspObj->res;
DPIF_CFARDetList *objIn = res->cfarRngDopSnrList;
DPIF_PointCloudCartesian *objOut = res->detObjOut;
DPIF_PointCloudSideInfo *objOutSideInfo = res->detObjOutSideInfo;
DPU_AoAProcDSP_StaticConfig *params = &aoaDspObj->params;
cmplx32ReIm_t *azimuthFftOut = (cmplx32ReIm_t *)res->scratch1Buff;
float lambdaOverDist = params->lambdaOverDist;
range = objIn[objInCfarIdx].rangeIdx * params->rangeStep;
/* Compensate for range bias */
range -= aoaDspObj->dynLocalCfg.compRxChanCfg.rangeBias;
if (range < 0)
{
range = 0;
}
if(maxIdx > (DPU_AOAPROCDSP_NUM_ANGLE_BINS/2 -1))
{
sMaxIdx = maxIdx - DPU_AOAPROCDSP_NUM_ANGLE_BINS;
}
else
{
sMaxIdx = maxIdx;
}
Wx = 2 * (float) sMaxIdx / DPU_AOAPROCDSP_NUM_ANGLE_BINS;
/* Check if it is within configured field of view */
x = range * Wx * (lambdaOverDist/2);
if (params->numVirtualAntElev > 0)
{
peakAzimIm = (float) azimuthFftOut[maxIdx].imag;
peakAzimRe = (float) azimuthFftOut[maxIdx].real;
peakElevIm = (float) res->elevationFftOut[maxIdx].imag;
peakElevRe = (float) res->elevationFftOut[maxIdx].real;
Wz = atan2(peakAzimIm * peakElevRe - peakAzimRe * peakElevIm,
peakAzimRe * peakElevRe + peakAzimIm * peakElevIm)/PI_ + (2 * Wx);
if (Wz > 1)
{
Wz = Wz - 2;
}
else if (Wz < -1)
{
Wz = Wz + 2;
}
/* Check if it is within configured field of view */
if((((lambdaOverDist/2)*Wz) < aoaDspObj->dynLocalCfg.fovAoaLocalCfg.minElevationSineVal) || (((lambdaOverDist/2)*Wz) > aoaDspObj->dynLocalCfg.fovAoaLocalCfg.maxElevationSineVal))
{
goto exit;
}
z = range * Wz * (lambdaOverDist/2);
/*record wz for debugging/testing*/
res->detObjElevationAngle[objOutIdx] = Wz;
limitScale = sqrt(1 - Wz*Wz);
}
else
{
z = 0;
limitScale = 1;
}
if((((lambdaOverDist/2)*Wx) < (limitScale * aoaDspObj->dynLocalCfg.fovAoaLocalCfg.minAzimuthSineVal)) ||
(((lambdaOverDist/2)*Wx) > (limitScale * aoaDspObj->dynLocalCfg.fovAoaLocalCfg.maxAzimuthSineVal)))
{
goto exit;
}
temp = range*range -x*x -z*z;
if (temp > 0)
{
y = sqrt(temp);
}
else
{
goto exit;
}
objOut[objOutIdx].x = x;
objOut[objOutIdx].y = y;
objOut[objOutIdx].z = z;
objOut[objOutIdx].velocity = params->dopplerStep * dopplerSignIdx;
objOutSideInfo[objOutIdx].noise = objIn[objInCfarIdx].noise;
objOutSideInfo[objOutIdx].snr = objIn[objInCfarIdx].snr;
res->detObj2dAzimIdx[objOutIdx] = maxIdx;
objOutIdx++;
exit:
return (objOutIdx);
}
Can you point me to a reference coordinate system so I know what X Y and Z represent relative to the radar?
Can you point me to a reference coordinate system so I know what X Y and Z represent relative to the radar?
