dss_data_path.h

Go to the documentation of this file.

 
#ifndef DSS_DATA_PATH_H
#define DSS_DATA_PATH_H
 
#include "common/detected_obj.h"
#include "common/mrr_config.h"
#include "common/app_cfg.h"
#include <ti/sysbios/knl/Semaphore.h>
 
 
#include <ti/common/sys_common.h>
#include <ti/common/mmwave_error.h>
#include <ti/drivers/adcbuf/ADCBuf.h>
#include <ti/drivers/edma/edma.h>
#include "EKF_XYZ_Consts.h"
 
#ifdef __cplusplus
extern "C" {
#endif
 
#define BYTES_PER_SAMP_1D (2*sizeof(int16_t))  /*16 bit real, 16 bit imaginary => 4 bytes */
#define BYTES_PER_SAMP_2D (2*sizeof(int32_t))  /*32 bit real, 32 bit imaginary => 8 bytes */
#define BYTES_PER_SAMP_DET sizeof(uint16_t) /*pre-detection matrix is 16 bit unsigned =>2 bytes*/
 
//DETECTION (CFAR-CA) related parameters
#define DET_THRESH_MULT 25
#define DET_THRESH_SHIFT 5 //DET_THRESH_MULT and DET_THRESH_SHIFT together define the CFAR-CA threshold
#define DET_GUARD_LEN 4 // this is the one sided guard lenght
#define DET_NOISE_LEN 16 //this is the one sided noise length
 
#define PI_ 3.1415926535897
#define ONE_Q15 (1 << 15)
#define ONE_Q19 (1 << 19)
#define ONE_Q8 (1 << 8)
 
#define MMWDEMO_MEMORY_ALLOC_DOUBLE_WORD_ALIGN 8
#define MMWDEMO_MEMORY_ALLOC_MAX_STRUCT_ALIGN  sizeof(uint64_t)
 
#define MMW_PEAK_GROUPING_DET_MATRIX_BASED 1
 
#define MMW_PEAK_GROUPING_CFAR_PEAK_BASED  2
 
#define MMW_NOISE_AVG_MODE_CFAR_CA       ((uint8_t)0U)
 
#define MMW_NOISE_AVG_MODE_CFAR_CAGO     ((uint8_t)1U)
 
#define MMW_NOISE_AVG_MODE_CFAR_CASO     ((uint8_t)2U)
 
#define  MMWDEMO_OUTPUT_MSG_CLUSTERS        2
#define  MMWDEMO_OUTPUT_MSG_TRACKED_OBJECTS 3
#define  MMWDEMO_OUTPUT_MSG_PARKING_ASSIST  4
 
typedef struct cycleLog_t_ {
    uint32_t interChirpProcessingTime;  
    uint32_t interChirpWaitTime;        
    uint32_t interFrameProcessingTime;  
    uint32_t interFrameWaitTime;        
} cycleLog_t;
 
typedef struct maxVelEnhStruct_t_
{
    float velResolutionFastChirp;
    float invVelResolutionSlowChirp;
    uint16_t maxVelAssocThresh;
    uint16_t padding;
}maxVelEnhStruct_t;
 
    
typedef struct MmwDemo_objRaw1D
{
    uint16_t   rangeIdx;     
    uint16_t   dopplerIdx;   
    int16_t   velDisambFacValidity; 
    uint16_t   dopplerSNRdB;      
} MmwDemo_objRaw1D_t;
 
 
 
typedef struct MmwDemo_objRaw2D
{
    uint16_t   rangeIdx;     
    uint16_t   dopplerIdx;   
    uint16_t   range;        
    int16_t   speed;        
    uint16_t   peakVal;      
    uint16_t   rangeSNRdB;     
    uint16_t   dopplerSNRdB;   
} MmwDemo_objRaw2D_t;
 
typedef struct MmwDemo_detectedObjActual_t
{
    uint16_t   rangeIdx;            
    uint16_t   dopplerIdx;          
    uint16_t   range;               
    int16_t    speed;               
    int16_t    sinAzim;             
    uint16_t   peakVal;             
    uint16_t   rangeSNRdB;          
    uint16_t   dopplerSNRdB;        
    uint16_t   sinAzimSNRLin;       
    int16_t  x;             
    int16_t  y;             
    int16_t  z;             
} MmwDemo_detectedObjActual;
 
 
typedef struct clusteringDBscanReportForTx_t
{
    int16_t     xCenter;               
    int16_t     yCenter;               
    int16_t     xSize;                 
    int16_t     ySize;                 
} clusteringDBscanReportForTx;
 
typedef struct trackingReportForTx_t
{
    int16_t     x;                  
    int16_t     y;                  
    int16_t     xd;                 
    int16_t     yd;                 
    int16_t     xSize;              
    int16_t     ySize;              
} trackingReportForTx;
 
typedef struct MmwDemo_detectedObjForTx_t
{
    int16_t   speed;        
    uint16_t   peakVal;     
    int16_t  x;             
    int16_t  y;             
    int16_t  z;             
#ifdef SEND_SNR_INFO
    uint16_t   rangeSNRdB;          
    uint16_t   dopplerSNRdB;        
#endif
} MmwDemo_detectedObjForTx;
 
 
 
typedef struct SNRThresholds
{
    uint16_t   rangelim;        
    uint16_t   threshold;        
} RangeDependantThresh_t;
 
 
typedef struct MmwDemo_1D_DopplerLines
{
    uint32_t   *dopplerLineMask;      
    uint16_t   currentIndex;     
    uint16_t    dopplerLineMaskLen;   
} MmwDemo_1D_DopplerLines_t;
 
typedef struct MmwDemo_timingInfo
{
    uint32_t interFrameProcCycles;
 
    uint32_t transmitOutputCycles;
 
    uint32_t chirpProcessingEndTime;
 
    uint32_t chirpProcessingEndMarginMin;
 
    uint32_t chirpProcessingEndMarginMax;
 
    uint32_t interFrameProcessingEndTime;
 
    uint32_t interFrameProcessingEndMargin;
 
    uint32_t activeFrameCPULoad; 
 
    uint32_t interFrameCPULoad; 
 
} MmwDemo_timingInfo_t;
 
 
#define DBSCAN_ERROR_CODE_OFFSET 100
typedef enum
{
    DBSCAN_OK = 0,                                                  
    DBSCAN_ERROR_MEMORY_ALLOC_FAILED = DBSCAN_ERROR_CODE_OFFSET,    
    DBSCAN_ERROR_NOT_SUPPORTED,                                     
    DBSCAN_ERROR_CLUSTER_LIMIT_REACHED                              
} clusteringDBscanErrorCodes;
 
 
typedef struct clusteringDBscanInstance
{
    float       epsilon;
    float       weight;
    float       vFactor;
    uint16_t    minPointsInCluster;
    uint16_t    maxClusters;
    uint16_t    pointIndex;
 
    uint16_t    maxPoints;  // Maximum number of points that can be services per dbscanRun call
    uint16_t    dBScanNeighbourLim;   
    uint16_t     fixedPointScale;     
    char        *scratchPad;
    char        *visited;
    char        *scope;
    uint16_t    *neighbors;
    float       *distances;
    
} clusteringDBscanInstance_t;
 
 
typedef struct clusteringDBscanConfig
{
    float       epsilon;              
    float       weight;               
    float       vFactor;              
    uint16_t    minPointsInCluster;   
    uint16_t    maxClusters;          
    uint16_t     fixedPointScale;     
    uint16_t    maxPoints;            
} clusteringDBscanConfig_t;
 
 
typedef struct clusteringDBscanReport
{
    uint16_t    numPoints;             
    int16_t     xCenter;               
    int16_t     yCenter;               
    int16_t     xSize;                 
    int16_t     ySize;                 
    int16_t     avgVel;                
} clusteringDBscanReport_t;
 
typedef struct trackingInputReport
{
    int16_t     xSize;                 
    int16_t     ySize;                 
    float     measVec[3];               
    float     measCovVec[3];            
} trackingInputReport_t;
 
typedef struct clusteringDBscanOutput
{
    uint16_t *IndexArray;                       
    uint16_t numCluster;                        
    clusteringDBscanReport_t *report;           
} clusteringDBscanOutput_t;
 
 
 
#define TRACKER_SCRATCHPAD_FLT_SIZE (N_MEASUREMENTS + \
                                     N_UNIQ_ELEM_IN_SYM_RESIDCOVMAT + \
                                     N_UNIQ_ELEM_IN_SYM_RESIDCOVMAT + \
                                     N_UNIQ_ELEM_IN_HMAT + \
                                     (N_STATES*N_MEASUREMENTS) + \
                                     N_UNIQ_ELEM_IN_SYM_COVMAT + \
                                     (N_STATES*N_MEASUREMENTS) + \
                                     (N_STATES*N_STATES) + \
                                     N_UNIQ_ELEM_IN_SYM_COVMAT + \
                                     N_MEASUREMENTS)
#define TRACKER_SCRATCHPAD_SHORT_SIZE (MRR_MAX_OBJ_OUT + 2*MAX_TRK_OBJs)
 
typedef struct KFtrackerInstance
{
    float       rangeAssocThresh;
    float       velAssocThresh;
    float       azimAssocThresh;
    float       distAssocThreshSq;
    uint16_t    maxTrackers;
    uint16_t    fixedPointScale;
 
    /* Temporaries for tracking. In the '//comment' the size of the memory to be assigned is given. */
    float   * scratchPadFlt;        // TRACKER_SCRATCHPAD_FLT_SIZE
    int16_t * scratchPadShort;      // (MRR_MAX_OBJ_OUT + 2*MAX_TRK_OBJs)
    int16_t * assocMeasIndx;        // [MRR_MAX_OBJ_OUT];
    int16_t * freeTrackerIndxArray; //[MAX_TRK_OBJs];
    int16_t * selectedIndxArr;      // [MAX_TRK_OBJs];    
    float * stateVecRrd;            // [N_MEASUREMENTS];
    float * residCovmat;            // [N_UNIQ_ELEM_IN_SYM_RESIDCOVMAT];
    float * invResidCovmat;         // [N_UNIQ_ELEM_IN_SYM_RESIDCOVMAT];
    float * hMat;                   // [N_UNIQ_ELEM_IN_HMAT];
    float * kalmanGain;             //[N_STATES*N_MEASUREMENTS];
    float * covmattmp;              //  [N_UNIQ_ELEM_IN_SYM_COVMAT];
    float * kalmanGainTemp;         // [N_STATES*N_MEASUREMENTS];
    float * stateCovMattemp;        // [N_STATES*N_STATES];
    float * stateCovMattempP;       // [N_UNIQ_ELEM_IN_SYM_COVMAT];
    float * measResidual;           // N_MEASUREMENTS
} KFtrackerInstance_t;
 
typedef struct KFstate
{
    uint8_t age;
    uint8_t tick;
    uint16_t validity;
    int16_t xSize;
    int16_t ySize;
    float vec[N_STATES];
    float covmat[N_UNIQ_ELEM_IN_SYM_COVMAT];
}KFstate_t;
 
 
 
typedef struct DSS_DataPathObj_t
{
    ADCBuf_Handle adcbufHandle;
 
    EDMA_Handle edmaHandle[2];
 
    EDMA_errorInfo_t  EDMA_errorInfo;
 
    EDMA_transferControllerErrorInfo_t EDMA_transferControllerErrorInfo;
 
    cmplx16ReIm_t *ADCdataBuf;
 
    cmplx16ReIm_t *twiddle16x16_1D;
 
    int16_t *window1D;
 
    cmplx16ReIm_t *adcDataIn;
 
    cmplx16ReIm_t *fftOut1D;
 
    cmplx32ReIm_t *twiddle32x32_2D;
 
    int32_t *window2D;
 
    cmplx16ReIm_t *dstPingPong;
 
    cmplx32ReIm_t *windowingBuf2D;
 
    cmplx32ReIm_t *fftOut2D;
 
    uint16_t *log2Abs;
 
    uint16_t *sumAbs;
    
    uint16_t *sumAbsSlowChirp;
 
    uint16_t *sumAbsRange;
 
    uint16_t *cfarDetObjIndexBuf;
    
    uint16_t *cfarDetObjSNR;
 
    cmplx32ReIm_t *azimuthIn;
 
    cmplx32ReIm_t *antInp;
    
    cmplx32ReIm_t *elevationIn;
       
    cmplx32ReIm_t *azimuthOut;
    
    cmplx32ReIm_t *elevationOut;
 
    float   *azimuthMagSqr;
 
    cmplx32ReIm_t *azimuthTwiddle32x32;
 
    cmplx16ImRe_t *azimuthModCoefs;
 
    cmplx16ImRe_t *rxChPhaseComp;
    
    cmplx32ImRe_t *dcRangeSigMean;
 
    cmplx16ReIm_t *radarCube;
 
    uint16_t *detMatrix;
    
    uint8_t *dBscanScratchPad;
    
    uint16_t *dbscanOutputDataIndexArray;
    
    clusteringDBscanReport_t *dbscanOutputDataReport;
    
    trackingInputReport_t *trackingInput;
    
    float   * trackerScratchPadFlt;  
    
    int16_t * trackerScratchPadShort;
    
    uint8_t processingPath;
 
    uint8_t chirpThreshold;
 
    uint8_t dcRangeSigCalibCntr;
 
    uint8_t log2NumAvgChirps;
 
    uint8_t log2NumDopplerBins;
 
    uint8_t xyzOutputQFormat;
 
    uint8_t subframeIndx;
 
    uint8_t log2numVirtAnt;
 
    uint8_t sinAzimQFormat;
 
    uint8_t parkingAssistNumBins;
 
    uint8_t parkingAssistNumBinsLog2;
 
    uint8_t padding;
 
    uint16_t cfarCfgRange_minIndxToIgnoreHPF;
 
    uint16_t parkingAssistMaxRange;
 
    uint16_t parkingAssistMinRange;
 
    uint16_t numChirpsPerFrame;
 
    uint16_t numDetObj;
 
    uint16_t numDetObjRaw;
 
    uint16_t minRange;
    
    uint16_t maxRange;
 
    uint16_t numActiveTrackers;
 
    uint16_t maxNumObj2DRaw;
 
    uint16_t numRxAntennas;
 
    uint16_t numTxAntennas;
    
    uint16_t numVirtualAntennas;
 
    uint16_t numVirtualAntAzim;
 
    uint16_t numVirtualAntElev;
 
    uint16_t numAngleBins;
    
    uint16_t numChirpsPerChirpType;
    
    uint16_t numDopplerBins;
    
    uint16_t chirpCount;
 
    uint16_t txAntennaCount;
 
    uint16_t dopplerBinCount;
 
    uint16_t chirpTypeCount;
 
    uint16_t numAdcSamples;
 
    uint16_t numRangeBins;
    
    cmplx16ImRe_t azimuthModCoefsHalfBin;
    
    cmplx16ImRe_t azimuthModCoefsThirdBin;
    
    cmplx16ImRe_t azimuthModCoefsTwoThirdBin;
 
    float rangeResolution;
 
    float velResolution;
 
    float maxUnambiguousVel;
 
    float invOneQFormat;
 
    float invOneSinAzimFormat;
 
    float invNumAngleBins;
 
    RangeDependantThresh_t SNRThresholds[MAX_NUM_RANGE_DEPENDANT_SNR_THRESHOLDS] ;
    
    RangeDependantThresh_t peakValThresholds[MAX_NUM_RANGE_DEPENDANT_SNR_THRESHOLDS] ;
    
    MmwDemo_1D_DopplerLines_t detDopplerLines;
 
    MmwDemo_detectedObjActual *detObj2D;
 
    MmwDemo_objRaw1D_t *detObj1DRaw;
 
    MmwDemo_objRaw2D_t *detObj2DRaw;
 
    MmwDemo_CfarCfg cfarCfgDoppler;
 
    MmwDemo_CfarCfg cfarCfgRange;
 
    MmwDemo_MultiObjBeamFormingCfg multiObjBeamFormingCfg;
 
    MmwDemo_CalibDcRangeSigCfg calibDcRangeSigCfg;
 
    MmwDemo_timingInfo_t timingInfo;
    
    cycleLog_t cycleLog;
    
    maxVelEnhStruct_t maxVelEnhStruct;
    
    clusteringDBscanInstance_t dbScanInstance;
    
    clusteringDBscanOutput_t dbScanReport;
 
    KFtrackerInstance_t trackerInstance;   
    
    KFstate_t * trackerState; 
    
    float * trackerQvecList; 
    
    clusteringDBscanReport_t *dbScanState;
 
    MmwDemo_detectedObjForTx * detObjFinal;
    
    clusteringDBscanReportForTx * clusterOpFinal;
    
    trackingReportForTx * trackerOpFinal;
    
    uint16_t * parkingAssistBins;
    
    uint16_t * parkingAssistBinsState;
    
    uint16_t * parkingAssistBinsStateCnt;
        
} DSS_DataPathObj;
 
void MmwDemo_dataPathInit1Dstate(DSS_DataPathObj *obj);
 
void MmwDemo_dataPathDeleteSemaphore(DSS_DataPathObj *obj);
 
int32_t MmwDemo_dataPathInitEdma(DSS_DataPathObj *obj);
 
int32_t MmwDemo_dataPathCopyEdmaHandle(DSS_DataPathObj *objOutput, DSS_DataPathObj *objInput);
 
int32_t MmwDemo_dataPathConfigEdma(DSS_DataPathObj *obj);
 
void MmwDemo_dataPathConfigBuffers(DSS_DataPathObj *obj, uint32_t adcBufAddress);
 
void MmwDemo_dataPathConfigAzimuthHeatMap(DSS_DataPathObj *obj);
 
void MmwDemo_dataPathConfigFFTs(DSS_DataPathObj *obj);
 
void MmwDemo_waitEndOfChirps(DSS_DataPathObj *obj, uint8_t subframeIdx);
 
void MmwDemo_processChirp(DSS_DataPathObj *obj, uint8_t subframeIdx);
 
void MmwDemo_interFrameProcessing(DSS_DataPathObj *obj, uint8_t subframeIdx);
 
uint32_t MmwDemo_pow2roundup (uint32_t x);
 
float convertSNRdBToVar(uint16_t SNRdB,uint16_t bitW, uint16_t n_samples, float resolution);
 
float convertSNRLinToVar(uint16_t SNRLin,uint16_t bitW, uint16_t n_samples, float resolution);
 
void populateOutputs(DSS_DataPathObj *obj);
 
uint32_t pruneTrackingInput(trackingInputReport_t * trackingInput, uint32_t numCluster);
 
float quadraticInterpFltPeakLoc(float * restrict y, int32_t len, int32_t indx);
 
 
/*!*****************************************************************************************************************
 * \brief
 * Function Name       :    MmwDemo_DopplerCompensation
 *
 * \par
 * <b>Description</b>  : Compensation of Doppler phase shift in the virtual antennas,
 *                       (corresponding to second Tx antenna chirps). Symbols
 *                       corresponding to virtual antennas, are rotated by half
 *                       of the Doppler phase shift measured by Doppler FFT.
 *                       The phase shift read from the table using half of the
 *                       object Doppler index  value. If the Doppler index is
 *                       odd, an extra half of the bin phase shift is added.
 *
 * @param[in]               dopplerIdx     : Doppler index of the object
 *
 * @param[in]               numDopplerBins : Number of Doppler bins
 *
 * @param[in]               azimuthModCoefs: Table with cos/sin values SIN in even position, COS in odd position
 *                                           exp(1j*2*pi*k/N) for k=0,...,N-1 where N is number of Doppler bins.
 *
 * @param[out]              azimuthModCoefsHalfBin :  exp(1j*2*pi* 0.5 /N)
 *
 * @param[in,out]           azimuthIn        :Pointer to antenna symbols to be Doppler compensated
 *
 * @param[in]              numAnt       : Number of antenna symbols to be Doppler compensated
 *
 * @param[in]              numTxAnt       : Number of Tx antenna 
 *
 * @param[in]              txAntIdx       : Tx Antenna index (Tx1:0 Tx2:1 Tx3:2) 
 *
 * @return                  void
 *
 *******************************************************************************************************************
 */
void MmwDemo_addDopplerCompensation(int32_t dopplerIdx,
                                    int32_t numDopplerBins,
                                    uint32_t *azimuthModCoefs,
                                    uint32_t *azimuthModCoefsThirdBin,
                                    uint32_t *azimuthModCoefsTwoThirdBin,
                                    int64_t *azimuthIn,
                                    uint32_t numAnt,
                                    uint32_t numTxAnt,
                                    uint16_t txAntIdx);
void MmwDemo_XYcalc (DSS_DataPathObj *obj,
                     uint32_t objIndex,
                     uint16_t azimIdx,
                     float * azimuthMagSqr);
                     
                     
void MmwDemo_XYZcalc (DSS_DataPathObj *obj,
                    uint32_t objIndex,
                    uint16_t azimIdx,
                    float * azimuthMagSqr);
                     
void parkingAssistInit(DSS_DataPathObj *obj);
 
 
 
#ifdef __cplusplus
}
#endif
 
#endif /* DSS_DATA_PATH_H */