commit c986807e7512ba34ee24af0c0f3f21804d058344 Author: Hannes Date: Thu Apr 8 16:08:12 2021 +0200 initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bc13d7f --- /dev/null +++ b/.gitignore @@ -0,0 +1,40 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Folder +.settings/ +Debug/ +.xdchelp +.ccsproject +.cproject +.project diff --git a/mss/mss_main.c b/mss/mss_main.c new file mode 100644 index 0000000..bc91308 --- /dev/null +++ b/mss/mss_main.c @@ -0,0 +1,2587 @@ +/** + * @file mss_main.c + * + * @brief + * This is the main file which implements the millimeter wave Demo + * + * \par + * NOTE: + * (C) Copyright 2019 Texas Instruments, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** @mainpage 3D People Counting Demo for XWR68XX + * [TOC] + * @section intro_sec Introduction + * + * The 3D People Counting Demo shows the indoor people counting capabilities using XWR68xx SoC. + * The demo uses adapted range HWA DPU and DPC offerred by the mmWave SDK (Software Development Kit) + * for range processing on R4F and HWA, and uses advanced Capon beamforming on DSP to estimate + * range, Doppler, azimuth angle and elevation angle of indoor targets. + * + * Please refer to SDK documentation for range HWA DPU and DPC details. The additional adaptions are + * the following: + * - In rangeHWA DPU, modified range FFT scaling settings for HWA to maximize the SNR and dynamic + * range for indoor environment and the particular FFT size we use. + * - In rangeHWA DPC, added support for radar cube format 2. + * - Modified mmwdemo_rfparser.c to output chirp interval and frame period. Also fixed the error + * in Doppler step calculation. + * + * For Capon beamforming technique on DSP, please refer to document included in the packages for details. + * + * Please refer to the users guide for UART output format and chirp configuration details. + * + */ + +/************************************************************************** + *************************** Include Files ******************************** + **************************************************************************/ + +/* Standard Include Files. */ +#include +#include +#include +#include +#include +#include + + +/* BIOS/XDC Include Files. */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* mmWave SDK Include Files: */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Demo Include Files */ +#include +#include + +#include +#include +#include +#include + + +/* Profiler Include Files */ +#include +#include +#include +#include +#include +/** + * @brief Task Priority settings: + * Mmwave task is at higher priority because of potential async messages from BSS + * that need quick action in real-time. + * + * CLI task must be at a lower priority than object detection + * dpm task priority because the dynamic CLI command handling in the objection detection + * dpm task assumes CLI task is held back during this processing. The alternative + * is to use a semaphore between the two tasks. + */ +#define MMWDEMO_CLI_TASK_PRIORITY 2 +#define MMWDEMO_TRACKERDPU_TASK_PRIORITY 3 +#define MMWDEMO_UARTTX_TASK_PRIORITY 4 +#define MMWDEMO_DPC_OBJDET_DPM_TASK_PRIORITY 5 +#define MMWDEMO_MMWAVE_CTRL_TASK_PRIORITY 6 + +#if (MMWDEMO_CLI_TASK_PRIORITY >= MMWDEMO_DPC_OBJDET_DPM_TASK_PRIORITY) +#error CLI task priority must be < Object Detection DPM task priority +#endif + +#define DPC_OBJDET_R4F_INSTANCEID (0xFEEDFEED) +#define DEBUG(_x) _x + +/** + * @brief + * Global Variable for LDO BYPASS config, PLease consult your + * board/EVM user guide before changing the values here + */ +rlRfLdoBypassCfg_t gRFLdoBypassCfg = +{ + .ldoBypassEnable = 0, /* 1.0V RF supply 1 and 1.0V RF supply 2 */ + .supplyMonIrDrop = 0, /* IR drop of 3% */ + .ioSupplyIndicator = 0, /* 3.3 V IO supply */ +}; + +/************************************************************************** + *************************** Global Definitions *************************** + **************************************************************************/ + +/** + * @brief + * Global Variable for tracking information required by the mmw Demo + */ +Pcount3DDemo_MSS_MCB gMmwMssMCB; + +extern ObjDetObj *gObjDetObj; + + /*! TCMB RAM buffer for object detection DPC */ +#define MMWDEMO_OBJDET_LOCALRAM_SIZE (8U * 1024U) +uint8_t gDPCTCM[MMWDEMO_OBJDET_LOCALRAM_SIZE]; +#pragma DATA_SECTION(gDPCTCM, ".dpcLocalRam"); +#pragma DATA_ALIGN(gDPCTCM, 4); + +/*! L3 RAM buffer for object detection DPC */ +#pragma DATA_SECTION(gMmwL3, ".l3ram"); +uint8_t gMmwL3[MMWAVE_MSSUSED_L3RAM_SIZE]; + +/************************************************************************** + *************************** Extern Definitions *************************** + **************************************************************************/ + +extern void MmwDemo_CLIInit(uint8_t taskPriority); + +/************************************************************************** + ************************* Millimeter Wave Demo Functions prototype ************* + **************************************************************************/ + +/* MMW demo functions for datapath operation */ +static void Pcount3DDemo_dataPathOpen(void); +static int32_t Pcount3DDemo_dataPathConfig (void); +static void Pcount3DDemo_dataPathStart (void); +static void Pcount3DDemo_dataPathStop (void); +static void Pcount3DDemo_handleObjectDetResult(DPM_Buffer *ptrResult); +static void Pcount3DDemo_DPC_ObjectDetection_reportFxn +( + DPM_Report reportType, + uint32_t instanceId, + int32_t errCode, + uint32_t arg0, + uint32_t arg1 +); +#ifdef TRACKERPROC_EN +static void MmwDemo_trackerDPUTask(UArg arg0, UArg arg1); +#endif +static void MmwDemo_uartTxTask(UArg arg0, UArg arg1); +/*Pcount3DDemo_transmitProcessedOutput +( + UART_Handle uartHandle, + Pcount3DDemo_output_message_UARTpointCloud *result, + uint32_t frameIdx, + uint8_t subFrameIdx, + Pcount3DDemo_output_message_stats *timingInfo +);*/ +static int32_t Pcount3DDemo_DPM_ioctl_blocking +( + DPM_Handle handle, + uint32_t cmd, + void* arg, + uint32_t argLen +); +static int32_t Pcount3DDemo_processPendingDynamicCfgCommands(uint8_t subFrameIndx); + +static void Pcount3DDemo_initTask(UArg arg0, UArg arg1); +static void Pcount3DDemo_platformInit(Pcount3DDemo_platformCfg *config); + +/* Mmwave control functions */ +static void Pcount3DDemo_mmWaveCtrlTask(UArg arg0, UArg arg1); +static int32_t Pcount3DDemo_mmWaveCtrlStop (void); +static int32_t Pcount3DDemo_eventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload); + +/* external sleep function when in idle (used in .cfg file) */ +void Pcount3DDemo_sleep(void); + +/* Edma related functions */ +static void Pcount3DDemo_edmaInit(Pcount3DDemo_DataPathObj *obj, uint8_t instance); +static void Pcount3DDemo_edmaOpen(Pcount3DDemo_DataPathObj *obj, uint8_t instance); +static void Pcount3DDemo_EDMA_transferControllerErrorCallbackFxn(EDMA_Handle handle, + EDMA_transferControllerErrorInfo_t *errorInfo); +static void Pcount3DDemo_EDMA_errorCallbackFxn(EDMA_Handle handle, EDMA_errorInfo_t *errorInfo); + + +/************************************************************************** + ************************* Millimeter Wave Demo Functions ********************** + **************************************************************************/ + +/** + * @b Description + * @n + * EDMA driver init + * + * @param[in] obj Pointer to data path object + * @param[in] instance EDMA instance + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_edmaInit(Pcount3DDemo_DataPathObj *obj, uint8_t instance) +{ + int32_t errorCode; + + errorCode = EDMA_init(instance); + if (errorCode != EDMA_NO_ERROR) + { + System_printf ("Debug: EDMA instance %d initialization returned error %d\n", errorCode); + Pcount3DDemo_debugAssert (0); + return; + } + + memset(&obj->EDMA_errorInfo, 0, sizeof(obj->EDMA_errorInfo)); + memset(&obj->EDMA_transferControllerErrorInfo, 0, sizeof(obj->EDMA_transferControllerErrorInfo)); +} + +/** + * @b Description + * @n + * Call back function for EDMA CC (Channel controller) error as per EDMA API. + * Declare fatal error if happens, the output errorInfo can be examined if code + * gets trapped here. + */ +static void Pcount3DDemo_EDMA_errorCallbackFxn(EDMA_Handle handle, EDMA_errorInfo_t *errorInfo) +{ + gMmwMssMCB.dataPathObj.EDMA_errorInfo = *errorInfo; + Pcount3DDemo_debugAssert(0); +} + +/** + * @b Description + * @n + * Call back function for EDMA transfer controller error as per EDMA API. + * Declare fatal error if happens, the output errorInfo can be examined if code + * gets trapped here. + */ +static void Pcount3DDemo_EDMA_transferControllerErrorCallbackFxn +( + EDMA_Handle handle, + EDMA_transferControllerErrorInfo_t *errorInfo +) +{ + gMmwMssMCB.dataPathObj.EDMA_transferControllerErrorInfo = *errorInfo; + Pcount3DDemo_debugAssert(0); +} + +/** + * @b Description + * @n + * Open EDMA driver instance + * + * @param[in] obj Pointer to data path object + * @param[in] instance EDMA instance + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_edmaOpen(Pcount3DDemo_DataPathObj *obj, uint8_t instance) +{ + int32_t errCode; + EDMA_instanceInfo_t edmaInstanceInfo; + EDMA_errorConfig_t errorConfig; + + obj->edmaHandle = EDMA_open(instance, &errCode, &edmaInstanceInfo); + + if (obj->edmaHandle == NULL) + { + Pcount3DDemo_debugAssert (0); + return; + } + + errorConfig.isConfigAllEventQueues = true; + errorConfig.isConfigAllTransferControllers = true; + errorConfig.isEventQueueThresholdingEnabled = true; + errorConfig.eventQueueThreshold = EDMA_EVENT_QUEUE_THRESHOLD_MAX; + errorConfig.isEnableAllTransferControllerErrors = true; + errorConfig.callbackFxn = Pcount3DDemo_EDMA_errorCallbackFxn; + errorConfig.transferControllerCallbackFxn = Pcount3DDemo_EDMA_transferControllerErrorCallbackFxn; + if ((errCode = EDMA_configErrorMonitoring(obj->edmaHandle, &errorConfig)) != EDMA_NO_ERROR) + { + //System_printf("Error: EDMA_configErrorMonitoring() failed with errorCode = %d\n", errCode); + Pcount3DDemo_debugAssert (0); + return; + } +} + +/** + * @b Description + * @n + * Close EDMA driver instance + * + * @param[in] obj Pointer to data path object + * + * @retval + * Not Applicable. + */ +void Pcount3DDemo_edmaClose(Pcount3DDemo_DataPathObj *obj) +{ + EDMA_close(obj->edmaHandle); +} + +/** + * @b Description + * @n + * HWA driver init + * + * @param[in] obj Pointer to data path object + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_hwaInit(Pcount3DDemo_DataPathObj *obj) +{ + /* Initialize the HWA */ + HWA_init(); +} + +/** + * @b Description + * @n + * Open HWA driver instance + * + * @param[in] obj Pointer to data path object + * @param[in] socHandle SOC driver handle + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_hwaOpen(Pcount3DDemo_DataPathObj *obj, SOC_Handle socHandle) +{ + int32_t errCode; + + /* Open the HWA Instance */ + obj->hwaHandle = HWA_open(0, socHandle, &errCode); + if (obj->hwaHandle == NULL) + { + //System_printf("Error: Unable to open the HWA Instance err:%d\n",errCode); + Pcount3DDemo_debugAssert (0); + return; + } +} + +/** + * @b Description + * @n + * Close HWA driver instance + * + * @param[in] obj Pointer to data path object + * + * @retval + * Not Applicable. + */ +void Pcount3DDemo_hwaClose(Pcount3DDemo_DataPathObj *obj) +{ + int32_t errCode; + + /* Close the HWA Instance */ + errCode = HWA_close(obj->hwaHandle); + if (errCode != 0) + { + Pcount3DDemo_debugAssert (0); + return; + } +} + +/** + * @b Description + * @n + * Send assert information through CLI. + * + * @param[in] expression Expression for evaluation + * @param[in] file C file that caused assertion + * @param[in] line Line number in C fine that caused assertion + * + */ +void _Pcount3DDemo_debugAssert(int32_t expression, const char *file, int32_t line) +{ + if (!expression) { + CLI_write ("Exception: %s, line %d.\n",file,line); + } +} + +/** + * @b Description + * @n + * Utility function to set the pending state of configuration. + * + * @param[in] subFrameCfg Pointer to Sub-frame specific configuration + * @param[in] offset Configuration structure offset that uniquely identifies the + * configuration to set to the pending state. + * + * @retval None + */ +static void Pcount3DDemo_setSubFramePendingState(Pcount3DDemo_SubFrameCfg *subFrameCfg, uint32_t offset) +{ + switch (offset) + { + case PCOUNT3DDEMO_ADCBUFCFG_OFFSET: + subFrameCfg->isAdcBufCfgPending = 1; + break; + default: + Pcount3DDemo_debugAssert(0); + break; + } +} + + +/** + * @b Description + * @n + * Resets (clears) all pending static (non-dynamic) configuration + * + */ +void Pcount3DDemo_resetStaticCfgPendingState(void) +{ + uint8_t indx; + + for(indx = 0; indx < gMmwMssMCB.objDetCommonCfg.numSubFrames; indx++) + { + gMmwMssMCB.subFrameCfg[indx].isAdcBufCfgPending = 0; + } + +} + +/** + * @b Description + * @n + * Utility function to find out if all configuration (common and sub-frame + * specific dynamic config) is in pending state. + * + * @retval 1 if all configuration (common and sub-frame specific dynamic config) + * is in pending state, else return 0 + */ +uint8_t Pcount3DDemo_isAllCfgInPendingState(void) +{ + uint8_t indx, flag = 1; + + for(indx = 0; indx < gMmwMssMCB.objDetCommonCfg.numSubFrames; indx++) + { + flag = flag && (gMmwMssMCB.subFrameCfg[indx].isAdcBufCfgPending == 1); + } + + return(flag); +} + +/** + * @b Description + * @n + * Utility function to find out if all configuration (common and sub-frame + * specific dynamic config) is in non-pending (cleared) state. + * + * @retval 1 if all configuration (common and sub-frame specific dynamic config) + * is in non-pending state, else return 0 + */ +uint8_t Pcount3DDemo_isAllCfgInNonPendingState(void) +{ + uint8_t indx, flag = 1; + + for(indx = 0; indx < gMmwMssMCB.objDetCommonCfg.numSubFrames; indx++) + { + flag = flag && (gMmwMssMCB.subFrameCfg[indx].isAdcBufCfgPending == 0); + } + + return(flag); +} + +/** + * @b Description + * @n + * Utility function to apply configuration to specified sub-frame + * + * @param[in] srcPtr Pointer to configuration + * @param[in] offset Offset of configuration within the parent structure + * @param[in] size Size of configuration + * @param[in] subFrameNum Sub-frame Number (0 based) to apply to, broadcast to + * all sub-frames if special code MMWDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +void Pcount3DDemo_CfgUpdate(void *srcPtr, uint32_t offset, uint32_t size, int8_t subFrameNum) +{ + /* if subFrameNum undefined, broadcast to all sub-frames */ + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + memcpy((void *)((uint32_t) &gMmwMssMCB.subFrameCfg[indx] + offset), srcPtr, size); + Pcount3DDemo_setSubFramePendingState(&gMmwMssMCB.subFrameCfg[indx], offset); + } + } + else + { + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + memcpy((void *)((uint32_t) &gMmwMssMCB.subFrameCfg[subFrameNum] + offset), srcPtr, size); + Pcount3DDemo_setSubFramePendingState(&gMmwMssMCB.subFrameCfg[subFrameNum], offset); + } +} + +/** @brief Transmits detection data over UART +* +* @param[in] uartHandle UART driver handle +* @param[in] result Pointer to result from object detection DPC processing +* @param[in] timingInfo Pointer to timing information provided from core that runs data path +*/ +void MmwDemo_uartTxTask(UArg arg0, UArg arg1) +/*static void Pcount3DDemo_transmitProcessedOutput +( + UART_Handle uartHandle, + Pcount3DDemo_output_message_UARTpointCloud *result, + uint32_t frameIdx, + uint8_t subFrameIdx, + Pcount3DDemo_output_message_stats *timingInfo +)*/ +{ + UART_Handle uartHandle; + Pcount3DDemo_output_message_header header; + uint32_t tlvIdx = 0; + uint32_t packetLen, subFrameIdx, frameIdx; + uint16_t *headerPtr; + Pcount3DDemo_output_message_stats *timingInfo; + + Pcount3DDemo_output_message_UARTpointCloud *objOut; + uint32_t sum, n, targetListLength=0, targetIndexLength=0, presenceIndLength=0; + volatile uint32_t startTime; + Pcount3DDemo_output_message_tl tl; + + /* Clear message header */ + memset((void *)&header, 0, sizeof(Pcount3DDemo_output_message_header)); + + + /* Header: */ + header.platform = 0xA6843; + header.magicWord[0] = 0x0102; + header.magicWord[1] = 0x0304; + header.magicWord[2] = 0x0506; + header.magicWord[3] = 0x0708; + header.version = MMWAVE_SDK_VERSION_BUILD | + (MMWAVE_SDK_VERSION_BUGFIX << 8) | + (MMWAVE_SDK_VERSION_MINOR << 16) | + (MMWAVE_SDK_VERSION_MAJOR << 24); + + + /* wait for new message and process all the messages received from the peer */ + while(1) + { + uint32_t numTargets, numIndices; + uint8_t *tList; + uint8_t *tIndex; + + + Semaphore_pend(gMmwMssMCB.uartTxSemHandle, BIOS_WAIT_FOREVER); + startTime = Cycleprofiler_getTimeStamp(); + + tlvIdx = 0; + uartHandle = gMmwMssMCB.loggingUartHandle; + objOut = &(gMmwMssMCB.pointCloudToUart); + subFrameIdx = gMmwMssMCB.currSubFrameIdx; + timingInfo = &gMmwMssMCB.subFrameStats[subFrameIdx].outputStats; + frameIdx = gMmwMssMCB.frameStatsFromDSP->frameStartIntCounter; + + packetLen = sizeof(Pcount3DDemo_output_message_header); + header.chirpProcessingMargin = timingInfo->interChirpProcessingMargin; + header.frameProcessingTimeInUsec = timingInfo->frameProcessingTimeInUsec; + header.trackingProcessingTimeInUsec = gMmwMssMCB.trackerProcessingTimeInUsec; + header.uartSendingTimeInUsec = gMmwMssMCB.uartProcessingTimeInUsec; + numTargets = gMmwMssMCB.numTargets; + numIndices = gMmwMssMCB.numIndices; + tList = (uint8_t*)gMmwMssMCB.trackerOutput.tList[gMmwMssMCB.trackerOutput.currentDescr]; + tIndex = (uint8_t*)gMmwMssMCB.trackerOutput.tIndex[gMmwMssMCB.trackerOutput.currentDescr]; + + if (objOut->header.length > 0) + { + packetLen += objOut->header.length; + tlvIdx++; + } + if (numTargets > 0) + { + targetListLength = sizeof(Pcount3DDemo_output_message_tl) + numTargets*sizeof(trackerProc_Target); + packetLen += targetListLength; + tlvIdx++; + } + if ((numIndices > 0) && (numTargets > 0)) + { + targetIndexLength = sizeof(Pcount3DDemo_output_message_tl) + numIndices*sizeof(trackerProc_TargetIndex); + packetLen += targetIndexLength; + tlvIdx++; + } + if(gMmwMssMCB.presenceDetEnabled) + { + presenceIndLength = sizeof(Pcount3DDemo_output_message_tl) + sizeof(uint32_t);; + packetLen += presenceIndLength; + tlvIdx++; + } + + header.numTLVs = tlvIdx; + header.totalPacketLen = packetLen; + header.frameNumber = frameIdx; + header.subFrameNumber = subFrameIdx; + header.checkSum = 0; + + + headerPtr = (uint16_t *)&header; + for(n=0, sum = 0; n < sizeof(Pcount3DDemo_output_message_header)/sizeof(uint16_t); n++) + sum += *headerPtr++; + header.checkSum = ~((sum >> 16) + (sum & 0xFFFF)); + + /* Send packet header */ + UART_write (uartHandle, + (uint8_t*)&header, + sizeof(Pcount3DDemo_output_message_header)); + + /* Send detected Objects */ + if (objOut->header.length > 0) + { + UART_write (uartHandle, + (uint8_t*)objOut, + objOut->header.length); + } + Task_sleep(1); + /*Send Tracker information*/ + if (numTargets > 0) + { + tl.type = MMWDEMO_OUTPUT_MSG_TARGET_LIST; + tl.length = targetListLength; + UART_write(uartHandle, (uint8_t*)&tl, sizeof(Pcount3DDemo_output_message_tl)); + UART_write(uartHandle, tList, targetListLength-sizeof(Pcount3DDemo_output_message_tl)); + GPIO_toggle(gMmwMssMCB.cfg.platformCfg.SensorStatusGPIO); + } + /*Send Tracker Index Information*/ + if ((numIndices > 0) && (numTargets > 0)) + { + tl.type = MMWDEMO_OUTPUT_MSG_TARGET_INDEX; + tl.length = targetIndexLength; + UART_write(uartHandle, (uint8_t*)&tl, sizeof(Pcount3DDemo_output_message_tl)); + UART_write(uartHandle, tIndex, targetIndexLength-sizeof(Pcount3DDemo_output_message_tl)); + } + + /* Send Presence TLV if presence detect is enabled */ + if(gMmwMssMCB.presenceDetEnabled) + { + tl.type = MMWDEMO_OUTPUT_PRESENCE_IND; + tl.length = presenceIndLength; + + UART_write(uartHandle, (uint8_t*)&tl, sizeof(Pcount3DDemo_output_message_tl)); + UART_write(uartHandle, (uint8_t*)&(gMmwMssMCB.presenceInd), sizeof(uint32_t)); + } + + gMmwMssMCB.uartProcessingTimeInUsec = (Cycleprofiler_getTimeStamp() - startTime)/R4F_CLOCK_MHZ; + } +} + + + +/************************************************************************** + ******************** Millimeter Wave Demo control path Functions ***************** + **************************************************************************/ +/** + * @b Description + * @n + * The function is used to trigger the Front end to stop generating chirps. + * + * @retval + * Not Applicable. + */ +static int32_t Pcount3DDemo_mmWaveCtrlStop (void) +{ + int32_t errCode = 0; + + DebugP_log0("App: Issuing MMWave_stop\n"); + + /* Stop the mmWave module: */ + if (MMWave_stop (gMmwMssMCB.ctrlHandle, &errCode) < 0) + { + MMWave_ErrorLevel errorLevel; + int16_t mmWaveErrorCode; + int16_t subsysErrorCode; + + /* Error/Warning: Unable to stop the mmWave module */ + MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode); + if (errorLevel == MMWave_ErrorLevel_ERROR) + { + /* Error: Display the error message: */ + System_printf ("Error: mmWave Stop failed [Error code: %d Subsystem: %d]\n", + mmWaveErrorCode, subsysErrorCode); + + /* Not expected */ + Pcount3DDemo_debugAssert(0); + } + else + { + /* Warning: This is treated as a successful stop. */ + System_printf ("mmWave Stop error ignored [Error code: %d Subsystem: %d]\n", + mmWaveErrorCode, subsysErrorCode); + } + } + + return errCode; +} + +/** + * @b Description + * @n + * The task is used to provide an execution context for the mmWave + * control task + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_mmWaveCtrlTask(UArg arg0, UArg arg1) +{ + int32_t errCode; + + while (1) + { + /* Execute the mmWave control module: */ + if (MMWave_execute (gMmwMssMCB.ctrlHandle, &errCode) < 0) + { + //System_printf ("Error: mmWave control execution failed [Error code %d]\n", errCode); + Pcount3DDemo_debugAssert (0); + } + } +} + +/************************************************************************** + ******************** Millimeter Wave Demo data path Functions ******************* + **************************************************************************/ + +/** + * @b Description + * @n + * Help function to make DPM_ioctl blocking until response is reported + * + * @retval + * Success -0 + * Failed <0 + */ +static int32_t Pcount3DDemo_DPM_ioctl_blocking +( + DPM_Handle handle, + uint32_t cmd, + void* arg, + uint32_t argLen +) +{ + int32_t retVal = 0; + + DebugP_log3("Pcount3DDemo_DPM_ioctl_blocking: cmd = %d, *arg = %d, len = %d\n", cmd, *(uint8_t *)arg, argLen); + retVal = DPM_ioctl(handle, + cmd, + arg, + argLen); + + if(retVal == 0) + { + /* Wait until ioctl completed */ + Semaphore_pend(gMmwMssMCB.DPMioctlSemHandle, BIOS_WAIT_FOREVER); + } + + return(retVal); +} + +/** + * @b Description + * @n + * Perform Data path driver open + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_dataPathOpen(void) +{ + gMmwMssMCB.adcBufHandle = MmwDemo_ADCBufOpen(gMmwMssMCB.socHandle); + if(gMmwMssMCB.adcBufHandle == NULL) + { + Pcount3DDemo_debugAssert(0); + } +} + + + +/** + * @b Description + * @n + * The function is used to configure the data path based on the chirp profile. + * After this function is executed, the data path processing will ready to go + * when the ADC buffer starts receiving samples corresponding to the chirps. + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_dataPathConfig (void) +{ + int32_t errCode; + MMWave_CtrlCfg *ptrCtrlCfg; + Pcount3DDemo_DPC_ObjDet_CommonCfg *objDetCommonCfgDSP; + Pcount3DDemo_SubFrameCfg *subFrameCfg; + int8_t subFrameIndx; + MmwDemo_RFParserOutParams RFparserOutParams; + DPC_ObjectDetectionRangeHWA_PreStartCfg objDetPreStartR4fCfg; + DPC_ObjectDetectionRangeHWA_StaticCfg *staticCfgR4F; + DPC_ObjectDetection_PreStartCfg objDetPreStartDspCfg; + DPC_ObjectDetectionRangeHWA_PreStartCommonCfg preStartCommonCfgHWA; + uint8_t radarCubeFormat = DPIF_RADARCUBE_FORMAT_2; + + /* Get data path object and control configuration */ + ptrCtrlCfg = &gMmwMssMCB.cfg.ctrlCfg; + + objDetCommonCfgDSP = &gMmwMssMCB.objDetCommonCfg; + staticCfgR4F = &objDetPreStartR4fCfg.staticCfg; + + /* Get RF frequency scale factor */ + gMmwMssMCB.rfFreqScaleFactor = SOC_getDeviceRFFreqScaleFactor(gMmwMssMCB.socHandle, &errCode); + if (errCode < 0) + { + System_printf ("Error: Unable to get RF scale factor [Error:%d]\n", errCode); + Pcount3DDemo_debugAssert(0); + } + + objDetCommonCfgDSP->numSubFrames = MmwDemo_RFParser_getNumSubFrames(ptrCtrlCfg); + + DEBUG(System_printf("App: Issuing Pre-start Common Config IOCTL to R4F\n");) + + /* DPC pre-start common config */ + + DEBUG(System_printf("App: Issuing Pre-start Common Config IOCTL to DSP\n");) + errCode = Pcount3DDemo_DPM_ioctl_blocking (gMmwMssMCB.objDetDpmHandle, + DPC_OBJDET_IOCTL__STATIC_PRE_START_COMMON_CFG, + objDetCommonCfgDSP, + sizeof (Pcount3DDemo_DPC_ObjDet_CommonCfg)); + + if (errCode < 0) + { + System_printf ("Error: Unable to send DPC_OBJDET_IOCTL__STATIC_PRE_START_COMMON_CFG [Error:%d]\n", errCode); + goto exit; + } + + preStartCommonCfgHWA.numSubFrames = objDetCommonCfgDSP->numSubFrames; + errCode = Pcount3DDemo_DPM_ioctl_blocking (gMmwMssMCB.objDetDpmHandle, + DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_COMMON_CFG, + &preStartCommonCfgHWA, + sizeof (preStartCommonCfgHWA)); + if (errCode < 0) + { + System_printf ("Error: Unable to send DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_COMMON_CFG [Error:%d]\n", errCode); + goto exit; + } + + /* Reason for reverse loop is that when sensor is started, the first sub-frame + * will be active and the ADC configuration needs to be done for that sub-frame + * before starting (ADC buf hardware does not have notion of sub-frame, it will + * be reconfigured every sub-frame). This cannot be alternatively done by calling + * the Pcount3DDemo_ADCBufConfig function only for the first sub-frame because this is + * a utility API that computes the rxChanOffset that is part of ADC dataProperty + * which will be used by range DPU and therefore this computation is required for + * all sub-frames. + */ + for(subFrameIndx = objDetCommonCfgDSP->numSubFrames -1; subFrameIndx >= 0; + subFrameIndx--) + { + subFrameCfg = &gMmwMssMCB.subFrameCfg[subFrameIndx]; + + /***************************************************************************** + * Data path :: Algorithm Configuration + *****************************************************************************/ + + DEBUG(System_printf("App: Calling MmwDemo_RFParser_parseConfig\n");) + /* Parse the profile and chirp configs and get the valid number of TX Antennas */ + errCode = MmwDemo_RFParser_parseConfig(&RFparserOutParams, subFrameIndx, + &gMmwMssMCB.cfg.openCfg, ptrCtrlCfg, + &subFrameCfg->adcBufCfg, + gMmwMssMCB.rfFreqScaleFactor, + 0); + + if (errCode != 0) + { + System_printf ("Error: MmwDemo_RFParser_parseConfig [Error:%d]\n", errCode); + goto exit; + } + + /* The following code is to enable processing for number of doppler chirps that are + * less than 16 (the minimal numDopplerBins supported in doppler DPU DSP). + * In this case, interpolate to detect better with CFAR tuning. E.g. a 2 -pt FFT will + * be problematic in terms of distinguishing direction of motion */ + if (RFparserOutParams.numDopplerChirps <= 8) + { + RFparserOutParams.dopplerStep = RFparserOutParams.dopplerStep / (16 / RFparserOutParams.numDopplerBins); + RFparserOutParams.numDopplerBins = 16; + } + + /* Workaround for range DPU limitation for FFT size 1024 and 12 virtual antennas case*/ + if ((RFparserOutParams.numVirtualAntennas == 12) && (RFparserOutParams.numRangeBins == 1024)) + { + RFparserOutParams.numRangeBins = 1022; + } + + subFrameCfg->numChirpsPerChirpEvent = RFparserOutParams.numChirpsPerChirpEvent; + subFrameCfg->adcBufChanDataSize = RFparserOutParams.adcBufChanDataSize; + subFrameCfg->numAdcSamples = RFparserOutParams.numAdcSamples; + subFrameCfg->numChirpsPerSubFrame = RFparserOutParams.numChirpsPerFrame; + subFrameCfg->numVirtualAntennas = RFparserOutParams.numVirtualAntennas; + + DEBUG(System_printf("App: Calling MmwDemo_ADCBufConfig\n");) + errCode = MmwDemo_ADCBufConfig(gMmwMssMCB.adcBufHandle, + gMmwMssMCB.cfg.openCfg.chCfg.rxChannelEn, + subFrameCfg->numChirpsPerChirpEvent, + subFrameCfg->adcBufChanDataSize, + &subFrameCfg->adcBufCfg, + &staticCfgR4F->ADCBufData.dataProperty.rxChanOffset[0]); + if (errCode < 0) + { + System_printf("Error: ADCBuf config failed with error[%d]\n", errCode); + Pcount3DDemo_debugAssert (0); + } + + if (errCode < 0) + { + goto exit; + } + + /* DPC pre-start config R4F HWA*/ + { + + /*********************************************************************** + Pre-start preparation for objdetR4FHWA + ***********************************************************************/ + objDetPreStartR4fCfg.subFrameNum = subFrameIndx; + + /* Fill static configuration */ + staticCfgR4F->ADCBufData.data = (void *)SOC_XWR68XX_MSS_ADCBUF_BASE_ADDRESS; + staticCfgR4F->ADCBufData.dataProperty.adcBits = 2; /* 16-bit */ + + /* only complex format supported */ + Pcount3DDemo_debugAssert(subFrameCfg->adcBufCfg.adcFmt == 0); + + if (subFrameCfg->adcBufCfg.iqSwapSel == 1) + { + staticCfgR4F->ADCBufData.dataProperty.dataFmt = DPIF_DATAFORMAT_COMPLEX16_IMRE; + } + else + { + staticCfgR4F->ADCBufData.dataProperty.dataFmt = DPIF_DATAFORMAT_COMPLEX16_REIM; + } + if (subFrameCfg->adcBufCfg.chInterleave == 0) + { + staticCfgR4F->ADCBufData.dataProperty.interleave = DPIF_RXCHAN_INTERLEAVE_MODE; + } + else + { + staticCfgR4F->ADCBufData.dataProperty.interleave = DPIF_RXCHAN_NON_INTERLEAVE_MODE; + } + staticCfgR4F->radarCubeFormat = radarCubeFormat; + + staticCfgR4F->ADCBufData.dataProperty.numAdcSamples = RFparserOutParams.numAdcSamples; + staticCfgR4F->ADCBufData.dataProperty.numChirpsPerChirpEvent = RFparserOutParams.numChirpsPerChirpEvent; + staticCfgR4F->ADCBufData.dataProperty.numRxAntennas = RFparserOutParams.numRxAntennas; + staticCfgR4F->ADCBufData.dataSize = RFparserOutParams.numRxAntennas * RFparserOutParams.numAdcSamples * sizeof(cmplx16ImRe_t); + staticCfgR4F->numChirpsPerFrame = RFparserOutParams.numChirpsPerFrame; + staticCfgR4F->numDopplerChirps = RFparserOutParams.numDopplerChirps; + staticCfgR4F->numRangeBins = RFparserOutParams.numRangeBins; + staticCfgR4F->numTxAntennas = RFparserOutParams.numTxAntennas; + staticCfgR4F->numVirtualAntennas = RFparserOutParams.numVirtualAntennas; + + if (RFparserOutParams.numRangeBins >= 1022) + { + staticCfgR4F->rangeFFTtuning.fftOutputDivShift = 1; + /*scale 2 stages */ + staticCfgR4F->rangeFFTtuning.numLastButterflyStagesToScale = 2; + } + else if (RFparserOutParams.numRangeBins == 512) + { + staticCfgR4F->rangeFFTtuning.fftOutputDivShift = 2; + /*scale last stages */ + staticCfgR4F->rangeFFTtuning.numLastButterflyStagesToScale = 1; + } + else + { + /*NS: Changed this value to 2 to match ming's version */ + //staticCfgR4F->rangeFFTtuning.fftOutputDivShift = 3; + staticCfgR4F->rangeFFTtuning.fftOutputDivShift = 2; + + /*no scaling needed as ADC data is 16-bit and we have 8 bits to grow */ + staticCfgR4F->rangeFFTtuning.numLastButterflyStagesToScale = 0; + } + + /* Fill dynamic configuration for the sub-frame */ + objDetPreStartR4fCfg.dynCfg = subFrameCfg->objDetDynCfg.r4fDynCfg; + + DebugP_log1("App: Issuing Pre-start Config IOCTL (subFrameIndx = %d)\n", subFrameIndx); + + /* send pre-start config to R4F chain, using blocking call here */ + errCode = Pcount3DDemo_DPM_ioctl_blocking (gMmwMssMCB.objDetDpmHandle, + DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG, + &objDetPreStartR4fCfg, + sizeof (DPC_ObjectDetectionRangeHWA_PreStartCfg)); + if (errCode < 0) + { + System_printf ("Error: Unable to send DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG [Error:%d]\n", errCode); + goto exit; + } + DebugP_log0("App: DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG is processed \n"); + } + + /* DPC pre-start config DSP*/ + DEBUG(System_printf("App: pre-start config for DSP \n");) + { + DPU_radarProcessConfig_t *pParam_s; + + /*********************************************************************** + Pre-start preparation for objdetdsp + ***********************************************************************/ + /* Reset preStart config */ + memset((void *)&objDetPreStartDspCfg, 0, sizeof(DPC_ObjectDetection_PreStartCfg)); + + /* DPC configuration */ + objDetPreStartDspCfg.subFrameNum = subFrameIndx; + objDetPreStartDspCfg.dynCfg = subFrameCfg->objDetDynCfg.dspDynCfg; + pParam_s = &objDetPreStartDspCfg.dynCfg.caponChainCfg; + memcpy((void *)pParam_s, &(gMmwMssMCB.subFrameCfg[subFrameIndx].objDetDynCfg.dspDynCfg.caponChainCfg), sizeof(DPU_radarProcessConfig_t)); + + pParam_s->numRangeBins = RFparserOutParams.numRangeBins; + pParam_s->numTxAntenna = RFparserOutParams.numTxAntennas; + pParam_s->numPhyRxAntenna = RFparserOutParams.numRxAntennas; + pParam_s->numAntenna = pParam_s->numTxAntenna * pParam_s->numPhyRxAntenna; + if (pParam_s->numTxAntenna > 1) + pParam_s->mimoModeFlag = 1; + else + pParam_s->mimoModeFlag = 0; + pParam_s->numAdcSamplePerChirp = RFparserOutParams.numAdcSamples; + pParam_s->dynamicCfarConfig.rangeRes = RFparserOutParams.rangeStep; + pParam_s->staticCfarConfig.rangeRes = pParam_s->dynamicCfarConfig.rangeRes; + pParam_s->numChirpPerFrame = RFparserOutParams.numDopplerChirps ; + gMmwMssMCB.subFrameCfg[subFrameIndx].objDetDynCfg.dspDynCfg.caponChainCfg.numChirpPerFrame = RFparserOutParams.numDopplerChirps ; + pParam_s->framePeriod = RFparserOutParams.framePeriod; + pParam_s->chirpInterval = RFparserOutParams.chirpInterval; + pParam_s->bandwidth = RFparserOutParams.bandwidth; + pParam_s->centerFreq = RFparserOutParams.centerFreq; + gMmwMssMCB.subFrameCfg[subFrameIndx].objDetDynCfg.dspDynCfg.caponChainCfg.chirpInterval = RFparserOutParams.chirpInterval; + gMmwMssMCB.subFrameCfg[subFrameIndx].objDetDynCfg.dspDynCfg.caponChainCfg.framePeriod = RFparserOutParams.framePeriod; + gMmwMssMCB.subFrameCfg[subFrameIndx].objDetDynCfg.dspDynCfg.caponChainCfg.bandwidth = RFparserOutParams.bandwidth; + gMmwMssMCB.subFrameCfg[subFrameIndx].objDetDynCfg.dspDynCfg.caponChainCfg.centerFreq = RFparserOutParams.centerFreq; + + pParam_s->dynamicCfarConfig.dopplerRes = RFparserOutParams.dopplerStep; + pParam_s->dynamicCfarConfig.cfarType = RADARDEMO_DETECTIONCFAR_RA_CASOCFAR; //hardcoded, only method can be used in this chain + pParam_s->dynamicCfarConfig.inputType = RADARDEMO_DETECTIONCFAR_INPUTTYPE_SP; //hardcoded, only method can be used in this chain + pParam_s->staticCfarConfig.cfarType = RADARDEMO_DETECTIONCFAR_RA_CASOCFAR; //hardcoded, only method can be used in this chain + pParam_s->staticCfarConfig.inputType = RADARDEMO_DETECTIONCFAR_INPUTTYPE_SP; //hardcoded, only method can be used in this chain + pParam_s->maxNumDetObj = (uint16_t)MAX_RESOLVED_OBJECTS_PER_FRAME; + + /* The L3 memory and radarCube memory usage are reported and saved in @ref Pcount3DDemo_DPC_ObjectDetection_reportFxn. + The memory information is configured here and passed to objdetdsp chain. + */ + objDetPreStartDspCfg.dynCfg.radarCubeFormat = radarCubeFormat; + if(gMmwMssMCB.dataPathObj.radarCubeMem.addr != 0) + { + /* Update DPC radar cube configuration */ + objDetPreStartDspCfg.shareMemCfg.radarCubeMem.addr = gMmwMssMCB.dataPathObj.radarCubeMem.addr; + objDetPreStartDspCfg.shareMemCfg.radarCubeMem.size = gMmwMssMCB.dataPathObj.radarCubeMem.size; + + /* Update DPC L3 RAM configuration */ + objDetPreStartDspCfg.shareMemCfg.L3Ram.addr = (void *)((uint32_t)(gMmwMssMCB.dataPathObj.radarCubeMem.addr) + + gMmwMssMCB.dataPathObj.memUsage.L3RamUsage); + objDetPreStartDspCfg.shareMemCfg.L3Ram.size =gMmwMssMCB.dataPathObj.memUsage.L3RamTotal - gMmwMssMCB.dataPathObj.memUsage.L3RamUsage; + + /* Convert address for DSP core */ + objDetPreStartDspCfg.shareMemCfg.radarCubeMem.addr = (void *) SOC_translateAddress((uint32_t)objDetPreStartDspCfg.shareMemCfg.radarCubeMem.addr, + SOC_TranslateAddr_Dir_TO_OTHER_CPU, + &errCode); + DebugP_assert ((uint32_t)objDetPreStartDspCfg.shareMemCfg.radarCubeMem.addr != SOC_TRANSLATEADDR_INVALID); + + objDetPreStartDspCfg.shareMemCfg.L3Ram.addr = (void *) SOC_translateAddress((uint32_t)objDetPreStartDspCfg.shareMemCfg.L3Ram.addr, + SOC_TranslateAddr_Dir_TO_OTHER_CPU, + &errCode); + DebugP_assert ((uint32_t)objDetPreStartDspCfg.shareMemCfg.L3Ram.addr != SOC_TRANSLATEADDR_INVALID); + + /* Enable shared memory configuration */ + objDetPreStartDspCfg.shareMemCfg.shareMemEnable = true; + } + + /* send pre-start config */ + DEBUG(System_printf("App: pre-start config ready to be set to DSP \n");) + errCode = Pcount3DDemo_DPM_ioctl_blocking (gMmwMssMCB.objDetDpmHandle, + DPC_OBJDET_IOCTL__STATIC_PRE_START_CFG, + &objDetPreStartDspCfg, + sizeof (DPC_ObjectDetection_PreStartCfg)); + DebugP_log0("App: DPC_OBJDET_IOCTL__STATIC_PRE_START_CFG is processed \n"); + + if (errCode < 0) + { + System_printf ("Error: Unable to send DPC_OBJDET_IOCTL__STATIC_PRE_START_CFG [Error:%d]\n", errCode); + goto exit; + } + } + + + } +exit: + return errCode; +} +#ifdef TRACKERPROC_EN +/** + * @b Description + * @n + * The function is used to configure the tracker DPU. + * + * @retval + * 0 if no error, error code otherwise. + */ +int32_t MmwDemo_trackerConfig (void) +{ + int32_t errCode; + + DebugP_log0("App: Issuing Tracker Static Config IOCTL\n"); + + /* DPC pre-start common config */ + //errCode = DPM_ioctl (dataPathObj->objDetDpmHandle, + errCode = DPM_ioctl (gMmwMssMCB.objDetDpmHandle, + DPC_OBJDETRANGEHWA_IOCTL__STATIC_TRACKER_CFG, + &(gMmwMssMCB.trackerCfg), + sizeof (DPC_ObjectDetection_TrackerConfig)); + + if (errCode < 0) + { + System_printf ("Error: Unable to send DPC_OBJDETRANGEHWA_IOCTL__TRACKER_STATIC_CFG [Error:%d]\n", errCode); + goto exit; + } + +exit: + return errCode; +} +#endif + +/** + * @b Description + * @n + * This function is used to start data path to handle chirps from front end. + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_dataPathStart (void) +{ + int32_t retVal; + int32_t pointCloudSize; + + DebugP_log0("App: Issuing DPM_start\n"); + + pointCloudSize = MAX_RESOLVED_OBJECTS_PER_FRAME * sizeof(DPIF_PointCloudSpherical); + gMmwMssMCB.pointCloudFromDSP = (DPIF_PointCloudSpherical *)MemoryP_ctrlAlloc(pointCloudSize, sizeof(float)); + gMmwMssMCB.pointCloudSideInfoFromDSP = (DPIF_PointCloudSideInfo *)MemoryP_ctrlAlloc(MAX_RESOLVED_OBJECTS_PER_FRAME * sizeof(DPIF_PointCloudSideInfo), sizeof(int16_t)); + + /* Start the DPM Profile: */ + if ((retVal = DPM_start(gMmwMssMCB.objDetDpmHandle)) < 0) + { + /* Error: Unable to start the profile */ + System_printf("Error: Unable to start the DPM [Error: %d]\n", retVal); + Pcount3DDemo_debugAssert(0); + } + + /* Wait until start completed */ + Semaphore_pend(gMmwMssMCB.DPMstartSemHandle, BIOS_WAIT_FOREVER); + + DebugP_log0("App: DPM_start Done (post Semaphore_pend on reportFxn reporting start)\n"); +} + +/** + * @b Description + * @n + * This function is used to stop data path. + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_dataPathStop (void) +{ + int32_t retVal; + int32_t pointCloudSize; + + DebugP_log0("App: Issuing DPM_stop\n"); + + pointCloudSize = MAX_RESOLVED_OBJECTS_PER_FRAME * sizeof(Pcount3DDemo_output_message_point); + MemoryP_ctrlFree(gMmwMssMCB.pointCloudFromDSP, pointCloudSize); + + retVal = DPM_stop (gMmwMssMCB.objDetDpmHandle); + if (retVal < 0) + { + System_printf ("DPM_stop failed[Error code %d]\n", retVal); + Pcount3DDemo_debugAssert(0); + } +} + +/** + * @b Description + * @n + * Registered event function to mmwave which is invoked when an event from the + * BSS is received. + * + * @param[in] msgId + * Message Identifier + * @param[in] sbId + * Subblock identifier + * @param[in] sbLen + * Length of the subblock + * @param[in] payload + * Pointer to the payload buffer + * + * @retval + * Always return 0 + */ +static int32_t Pcount3DDemo_eventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload) +{ + uint16_t asyncSB = RL_GET_SBID_FROM_UNIQ_SBID(sbId); + + /* Process the received message: */ + switch (msgId) + { + case RL_RF_ASYNC_EVENT_MSG: + { + /* Received Asychronous Message: */ + switch (asyncSB) + { + case RL_RF_AE_CPUFAULT_SB: + { + Pcount3DDemo_debugAssert(0); + break; + } + case RL_RF_AE_ESMFAULT_SB: + { + Pcount3DDemo_debugAssert(0); + break; + } + case RL_RF_AE_ANALOG_FAULT_SB: + { + Pcount3DDemo_debugAssert(0); + break; + } + case RL_RF_AE_INITCALIBSTATUS_SB: + { + rlRfInitComplete_t* ptrRFInitCompleteMessage; + uint32_t calibrationStatus; + + /* Get the RF-Init completion message: */ + ptrRFInitCompleteMessage = (rlRfInitComplete_t*)payload; + calibrationStatus = ptrRFInitCompleteMessage->calibStatus & 0xFFFU; + + /* Display the calibration status: */ + CLI_write ("Debug: Init Calibration Status = 0x%x\n", calibrationStatus); + break; + } + case RL_RF_AE_FRAME_TRIGGER_RDY_SB: + { + gMmwMssMCB.stats.frameTriggerReady++; + break; + } + case RL_RF_AE_MON_TIMING_FAIL_REPORT_SB: + { + gMmwMssMCB.stats.failedTimingReports++; + break; + } + case RL_RF_AE_RUN_TIME_CALIB_REPORT_SB: + { + gMmwMssMCB.stats.calibrationReports++; + break; + } + case RL_RF_AE_FRAME_END_SB: + { + gMmwMssMCB.stats.sensorStopped++; + DebugP_log0("App: BSS stop (frame end) received\n"); + + Pcount3DDemo_dataPathStop(); + break; + } + default: + { + System_printf ("Error: Asynchronous Event SB Id %d not handled\n", asyncSB); + break; + } + } + break; + } + /* Async Event from MMWL */ + case RL_MMWL_ASYNC_EVENT_MSG: + { + switch (asyncSB) + { + case RL_MMWL_AE_MISMATCH_REPORT: + { + /* link reports protocol error in the async report from BSS */ + Pcount3DDemo_debugAssert(0); + break; + } + case RL_MMWL_AE_INTERNALERR_REPORT: + { + /* link reports internal error during BSS communication */ + Pcount3DDemo_debugAssert(0); + break; + } + } + break; + } + default: + { + System_printf ("Error: Asynchronous message %d is NOT handled\n", msgId); + break; + } + } + return 0; +} + +/** + * @b Description + * @n + * DPM Registered Report Handler. The DPM Module uses this registered function to notify + * the application about DPM reports. + * + * @param[in] reportType + * Report Type + * @param[in] instanceId + * Instance Identifier which generated the report + * @param[in] errCode + * Error code if any. + * @param[in] arg0 + * Argument 0 interpreted with the report type + * @param[in] arg1 + * Argument 1 interpreted with the report type + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_DPC_ObjectDetection_reportFxn +( + DPM_Report reportType, + uint32_t instanceId, + int32_t errCode, + uint32_t arg0, + uint32_t arg1 +) +{ + /* Only errors are logged on the console: */ + if ((errCode != 0) ) + { + /* Error: Detected log on the console and die all errors are FATAL currently. */ + System_printf ("Error: DPM Report %d received with error:%d arg0:0x%x arg1:0x%x\n", + reportType, errCode, arg0, arg1); + DebugP_assert (0); + } + + /* Processing further is based on the reports received: This is the control of the profile + * state machine: */ + switch (reportType) + { + case DPM_Report_IOCTL: + { + /***************************************************************** + * DPC has been configured without an error: + * - This is an indication that the profile configuration commands + * went through without any issues. + *****************************************************************/ + DebugP_log1("App: DPM Report IOCTL, command = %d\n", arg0); + + if (arg0 == DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG) + { + DPC_ObjectDetectionRangeHWA_PreStartCfg *cfg; + DPC_ObjectDetectionRangeHWA_preStartCfg_memUsage *memUsage; + + cfg = (DPC_ObjectDetectionRangeHWA_PreStartCfg*)arg1; + + /* Save radar cube memory information, it will be passed to objdetdsp chain for inter-frame processing */ + memcpy((void *)&gMmwMssMCB.dataPathObj.radarCubeMem, (void *)&cfg->radarCubeMem, + sizeof(DPC_ObjectDetectionRangeHWA_preStartCfg_radarCubeMem)); + + /* Get memory usage and print the usage */ + memUsage = &cfg->memUsage; + + /* Save memory usage, it will be passed to objdetdsp chain for inter-frame processing */ + memcpy((void *)&gMmwMssMCB.dataPathObj.memUsage, (void *)memUsage, + sizeof(DPC_ObjectDetectionRangeHWA_preStartCfg_memUsage)); + + System_printf("============ Heap Memory Stats ============\n"); + System_printf("%20s %12s %12s %12s %12s\n", " ", "Size", "Used", "Free", "DPCUsed"); + System_printf("%20s %12d %12d %12d %12d\n", "System Heap(TCMB)", + memUsage->SystemHeapTotal, memUsage->SystemHeapUsed, + memUsage->SystemHeapTotal - memUsage->SystemHeapUsed, + memUsage->SystemHeapDPCUsed); + + System_printf("%20s %12d %12d %12d\n", "L3", + memUsage->L3RamTotal, + memUsage->L3RamUsage, + memUsage->L3RamTotal - memUsage->L3RamUsage); + + System_printf("%20s %12d %12d %12d\n", "localRam(TCMB)", + memUsage->CoreLocalRamTotal, + memUsage->CoreLocalRamUsage, + memUsage->CoreLocalRamTotal - memUsage->CoreLocalRamUsage); + } + + switch(arg0) + { + /* The following ioctls take longer time to finish. It causes DPM to queue IOCTL requests on DSS before + * they are handled. However DPM has limited pipe queues, hence adding sync points in demo to avoid + * sending too many such ioctls to DSS at a time. + * The semaphore blocks CLI task to wait for the response from DSS before sending the next ioctl. + */ + case DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_COMMON_CFG: + case DPC_OBJDET_IOCTL__STATIC_PRE_START_COMMON_CFG: + case DPC_OBJDET_IOCTL__STATIC_PRE_START_CFG: +#ifdef TRACKERPROC_EN + case DPC_OBJDETRANGEHWA_IOCTL__STATIC_TRACKER_CFG: +#endif + /* The following ioctl returns memory information needs to be used in configuration follows the ioctl. + * The semaphore blocks CLI task to wiat for the response from DPC before further execution. + */ + case DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG: + System_printf("DPM IOCTL report msg = %d\n", arg0 ); + Semaphore_post(gMmwMssMCB.DPMioctlSemHandle); + break; + default: + break; + } + break; + } + case DPM_Report_DPC_STARTED: + { + /***************************************************************** + * DPC has been started without an error: + * - notify sensor management task that DPC is started. + *****************************************************************/ + DebugP_log0("App: DPM Report DPC Started\n"); + gMmwMssMCB.stats.dpmStartEvents++; + /* every sensor start should cause 2 DPM start events due to distributed domain + Wait for both the events before proceeding with remaining steps */ + if (gMmwMssMCB.stats.dpmStartEvents % 2 == 0) + { + Semaphore_post(gMmwMssMCB.DPMstartSemHandle); + } + break; + } + case DPM_Report_NOTIFY_DPC_RESULT: + { + /***************************************************************** + * datapath has finished frame processing, results are reported + *****************************************************************/ + DPM_Buffer* ptrResult; + + /* Get the result: */ + ptrResult = (DPM_Buffer*)arg0; + + Pcount3DDemo_handleObjectDetResult(ptrResult); + break; + } + case DPM_Report_DPC_ASSERT: + { + DPM_DPCAssert* ptrAssert; + + /***************************************************************** + * DPC Fault has been detected: + * - This implies that the DPC has crashed. + * - The argument0 points to the DPC assertion information + *****************************************************************/ + ptrAssert = (DPM_DPCAssert*)arg0; + CLI_write("Obj Det DPC Exception: %s, line %d.\n", ptrAssert->fileName, + ptrAssert->lineNum); + break; + } + case DPM_Report_DPC_STOPPED: + { + /***************************************************************** + * DPC has been stopped without an error: + * - This implies that the DPC can either be reconfigured or + * restarted. + *****************************************************************/ + DebugP_log0("App: DPM Report DPC Stopped\n"); + gMmwMssMCB.stats.dpmStopEvents++; + /* every sensor stop should cause 2 DPM stop events due to distributed domain + Wait for both the events before proceeding with remaining steps */ + if (gMmwMssMCB.stats.dpmStopEvents % 2 == 0) + { + Semaphore_post(gMmwMssMCB.DPMstopSemHandle); + } + break; + } + case DPM_Report_DPC_INFO: + case DPM_Report_NOTIFY_DPC_RESULT_ACKED: + { + /* Currently objDetDsp does not use this feature. */ + break; + } + default: + { + DebugP_assert (0); + break; + } + } + return; +} + +/** + * @b Description + * @n + * Utility function to get next sub-frame index + * + * @param[in] currentIndx Current sub-frame index + * @param[in] numSubFrames Number of sub-frames + * + * @retval + * Index of next sub-frame. + */ +static uint8_t Pcount3DDemo_getNextSubFrameIndx(uint8_t currentIndx, uint8_t numSubFrames) +{ + uint8_t nextIndx; + + if (currentIndx == (numSubFrames - 1)) + { + nextIndx = 0; + } + else + { + nextIndx = currentIndx + 1; + } + return(nextIndx); +} + +/** + * @b Description + * @n + * Utility function to get previous sub-frame index + * + * @param[in] currentIndx Current sub-frame index + * @param[in] numSubFrames Number of sub-frames + * + * @retval + * Index of previous sub-frame + */ +/* +static uint8_t Pcount3DDemo_getPrevSubFrameIndx(uint8_t currentIndx, uint8_t numSubFrames) +{ + uint8_t prevIndx; + + if (currentIndx == 0) + { + prevIndx = numSubFrames - 1; + } + else + { + prevIndx = currentIndx - 1; + } + return(prevIndx); +} +*/ +/** + * @b Description + * @n + * Processes any pending dynamic configuration commands for the specified + * sub-frame by fanning out to the respective DPUs using IOCTL interface, and + * resets (clears) the pending state after processing. + * + * @param[in] subFrameIndx Sub-frame index of desired sub-frame to process + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_processPendingDynamicCfgCommands(uint8_t subFrameIndx) +{ + int32_t retVal =0; + + return(retVal); +} + + +/** + * @b Description + * @n + * Function to handle frame processing results from DPC + * + * @param[in] ptrResult Pointer to DPC result + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_handleObjectDetResult +( + DPM_Buffer *ptrResult +) +{ + int32_t retVal; + + int32_t pntIdx; + DPC_ObjectDetection_ExecuteResultExportedInfo exportInfo; + DPC_ObjectDetection_ExecuteResult *dpcResults; + volatile uint32_t startTime; + uint8_t nextSubFrameIdx; + uint8_t numSubFrames; + uint8_t currSubFrameIdx; + Pcount3DDemo_SubFrameStats *currSubFrameStats; + float * heatmapBuff; + radarProcessOutput * outputFromDSP; + + /***************************************************************** + * datapath has finished frame processing, results are reported + *****************************************************************/ + + /* Validate DPC results buffer */ + DebugP_assert (ptrResult->size[0] == sizeof(DPC_ObjectDetection_ExecuteResult)); + + /* Translate the address: */ + dpcResults = (DPC_ObjectDetection_ExecuteResult *)SOC_translateAddress((uint32_t)ptrResult->ptrBuffer[0], + SOC_TranslateAddr_Dir_FROM_OTHER_CPU, + &retVal); + DebugP_assert ((uint32_t)dpcResults != SOC_TRANSLATEADDR_INVALID); + + /* Translate the address: */ + gMmwMssMCB.frameStatsFromDSP = (DPC_ObjectDetection_Stats *)SOC_translateAddress((uint32_t)ptrResult->ptrBuffer[1], + SOC_TranslateAddr_Dir_FROM_OTHER_CPU, + &retVal); + DebugP_assert ((uint32_t)(gMmwMssMCB.frameStatsFromDSP) != SOC_TRANSLATEADDR_INVALID); + + outputFromDSP = &(dpcResults->objOut); + + heatmapBuff = outputFromDSP->heatMapOut.data; + heatmapBuff = (float *)SOC_translateAddress((uint32_t)heatmapBuff, + SOC_TranslateAddr_Dir_FROM_OTHER_CPU, + &retVal); + //DebugP_log2("Pcount3DDemo_handleObjectDetResult: heatmap = (float *)0x%x, size = %d \n", (uint32_t)heatmapBuff, outputFromDSP->heatMapOut.dataSize ); + + gMmwMssMCB.heatMapOutFromDSP.dataSize = outputFromDSP->heatMapOut.dataSize; + gMmwMssMCB.heatMapOutFromDSP.data = outputFromDSP->heatMapOut.data; + + //copy to the format for output, and to future tracker + gMmwMssMCB.pointCloudToUart.header.length = sizeof(Pcount3DDemo_output_message_tl) + sizeof(Pcount3DDemo_output_message_point_unit) + sizeof(Pcount3DDemo_output_message_UARTpoint) * outputFromDSP->pointCloudOut.object_count; + if ( outputFromDSP->pointCloudOut.object_count == 0) + gMmwMssMCB.pointCloudToUart.header.length = 0; + gMmwMssMCB.pointCloudToUart.header.type = MMWDEMO_OUTPUT_MSG_POINT_CLOUD; + gMmwMssMCB.pointCloudToUart.pointUint.azimuthUnit = 0.01f; + gMmwMssMCB.pointCloudToUart.pointUint.elevationUnit = 0.01f; + gMmwMssMCB.pointCloudToUart.pointUint.rangeUnit = 0.00025f; + gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit = 0.00028f; + gMmwMssMCB.pointCloudToUart.pointUint.snrUint = 0.04f; + gMmwMssMCB.numDetectedPoints = outputFromDSP->pointCloudOut.object_count; + for (pntIdx = 0; pntIdx < (int32_t)outputFromDSP->pointCloudOut.object_count; pntIdx++ ) + { + //output to host + gMmwMssMCB.pointCloudToUart.point[pntIdx].azimuth = (int8_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].azimuthAngle/ gMmwMssMCB.pointCloudToUart.pointUint.azimuthUnit); + gMmwMssMCB.pointCloudToUart.point[pntIdx].elevation = (int8_t)round((outputFromDSP->pointCloudOut.pointCloud[pntIdx].elevAngle)/ gMmwMssMCB.pointCloudToUart.pointUint.elevationUnit); + gMmwMssMCB.pointCloudToUart.point[pntIdx].range = (uint16_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].range / gMmwMssMCB.pointCloudToUart.pointUint.rangeUnit); + gMmwMssMCB.pointCloudToUart.point[pntIdx].doppler = (int16_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].velocity / gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit); + gMmwMssMCB.pointCloudToUart.point[pntIdx].snr = (uint16_t)round((float)outputFromDSP->pointCloudOut.snr[pntIdx].snr * 0.125f / gMmwMssMCB.pointCloudToUart.pointUint.snrUint); + + //future tracker input + gMmwMssMCB.pointCloudFromDSP[pntIdx].azimuthAngle = outputFromDSP->pointCloudOut.pointCloud[pntIdx].azimuthAngle;// - gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sensorAzimuthTilt; + gMmwMssMCB.pointCloudFromDSP[pntIdx].elevAngle = outputFromDSP->pointCloudOut.pointCloud[pntIdx].elevAngle; + gMmwMssMCB.pointCloudFromDSP[pntIdx].range = outputFromDSP->pointCloudOut.pointCloud[pntIdx].range; + gMmwMssMCB.pointCloudFromDSP[pntIdx].velocity = outputFromDSP->pointCloudOut.pointCloud[pntIdx].velocity; + gMmwMssMCB.pointCloudSideInfoFromDSP[pntIdx].snr = (float)outputFromDSP->pointCloudOut.snr[pntIdx].snr * 0.125f; + } + + numSubFrames = gMmwMssMCB.objDetCommonCfg.numSubFrames; + currSubFrameIdx = dpcResults->subFrameIdx; + currSubFrameStats = &gMmwMssMCB.subFrameStats[currSubFrameIdx]; + gMmwMssMCB.currSubFrameIdx = currSubFrameIdx; + + /***************************************************************** + * Transmit results + *****************************************************************/ + startTime = Cycleprofiler_getTimeStamp(); + + currSubFrameStats->outputStats.interChirpProcessingMargin = gMmwMssMCB.subFrameStats[currSubFrameIdx].outputStats.interChirpProcessingMargin; + currSubFrameStats->outputStats.interFrameProcessingTime = gMmwMssMCB.frameStatsFromDSP->interFrameExecTimeInUsec; + currSubFrameStats->outputStats.interFrameCPULoad = (uint32_t) (100.f * (float) gMmwMssMCB.frameStatsFromDSP->interFrameExecTimeInUsec / gMmwMssMCB.subFrameCfg[currSubFrameIdx].objDetDynCfg.dspDynCfg.caponChainCfg.framePeriod); + currSubFrameStats->outputStats.frameProcessingTimeInUsec = gMmwMssMCB.frameStatsFromDSP->activeFrameProcTimeInUsec; + currSubFrameStats->outputStats.activeFrameCPULoad = (uint32_t) (100.f * (float) gMmwMssMCB.frameStatsFromDSP->activeFrameProcTimeInUsec / + (gMmwMssMCB.subFrameCfg[currSubFrameIdx].objDetDynCfg.dspDynCfg.caponChainCfg.framePeriod - gMmwMssMCB.subFrameCfg[currSubFrameIdx].objDetDynCfg.dspDynCfg.caponChainCfg.chirpInterval + * (float)gMmwMssMCB.subFrameCfg[currSubFrameIdx].objDetDynCfg.dspDynCfg.caponChainCfg.numChirpPerFrame) ) ; + + /***************************************************************** + * Send notification to data path after results are handled -- + * all data are local now, release the memory to DSP + *****************************************************************/ + /* Indicate result consumed and end of frame/sub-frame processing */ + exportInfo.subFrameIdx = currSubFrameIdx; + retVal = DPM_ioctl (gMmwMssMCB.objDetDpmHandle, + DPC_OBJDET_IOCTL__DYNAMIC_EXECUTE_RESULT_EXPORTED, + &exportInfo, + sizeof (DPC_ObjectDetection_ExecuteResultExportedInfo)); + if (retVal < 0) { + System_printf ("Error: DPM DPC_OBJDET_IOCTL__DYNAMIC_EXECUTE_RESULT_EXPORTED failed [Error code %d]\n", + retVal); + Pcount3DDemo_debugAssert (0); + } + + /* Run the tracker DPU */ + #ifdef TRACKERPROC_EN + /* Run the tracker DPU*/ + + Semaphore_post(gMmwMssMCB.trackerDPUSemHandle); + #endif + + /* Transmit processing results for the frame */ + Semaphore_post(gMmwMssMCB.uartTxSemHandle); + + /* + Pcount3DDemo_transmitProcessedOutput(gMmwMssMCB.loggingUartHandle, + &(gMmwMssMCB.pointCloudToUart), + frameStatsDSS->frameStartIntCounter, + dpcResults->subFrameIdx, + &currSubFrameStats->outputStats);*/ + + /* Update current frame transmit time */ + currSubFrameStats->outputStats.transmitOutputTime = (Cycleprofiler_getTimeStamp() - startTime)/R4F_CLOCK_MHZ; /* In micro seconds */ + + + /***************************************************************** + * Handle dynamic pending configuration + * For non-advanced frame case: + * process all pending dynamic config commands. + * For advanced-frame case: + * Process next sub-frame related pending dynamic config commands. + * If the next sub-frame was the first sub-frame of the frame, + * then process common (sub-frame independent) pending dynamic config + * commands. + *****************************************************************/ + startTime = Cycleprofiler_getTimeStamp(); + + nextSubFrameIdx = Pcount3DDemo_getNextSubFrameIndx(currSubFrameIdx, numSubFrames); + retVal = Pcount3DDemo_processPendingDynamicCfgCommands(nextSubFrameIdx); + if (retVal != 0) + { + System_printf ("Error: Executing Pending Dynamic Configuration Commands [Error code %d]\n", + retVal); + Pcount3DDemo_debugAssert (0); + } + currSubFrameStats->pendingConfigProcTime = (Cycleprofiler_getTimeStamp() - startTime)/R4F_CLOCK_MHZ; + + /***************************************************************** + * Prepare for subFrame switch + *****************************************************************/ + if(numSubFrames > 1) + { + Pcount3DDemo_SubFrameCfg *nextSubFrameCfg; + uint16_t dummyRxChanOffset[SYS_COMMON_NUM_RX_CHANNEL]; + + startTime = Cycleprofiler_getTimeStamp(); + + nextSubFrameCfg = &gMmwMssMCB.subFrameCfg[nextSubFrameIdx]; + + /* Configure ADC for next sub-frame */ + retVal = MmwDemo_ADCBufConfig(gMmwMssMCB.adcBufHandle, + gMmwMssMCB.cfg.openCfg.chCfg.rxChannelEn, + nextSubFrameCfg->numChirpsPerChirpEvent, + nextSubFrameCfg->adcBufChanDataSize, + &nextSubFrameCfg->adcBufCfg, + &dummyRxChanOffset[0]); + if(retVal < 0) + { + System_printf("Error: ADCBuf config failed with error[%d]\n", retVal); + Pcount3DDemo_debugAssert (0); + } + + currSubFrameStats->subFramePreparationTime = (Cycleprofiler_getTimeStamp() - startTime)/R4F_CLOCK_MHZ; + } + else + { + currSubFrameStats->subFramePreparationTime = 0; + } + + +} + + +#ifdef TRACKERPROC_EN +void MmwDemo_trackerDPUTask(UArg arg0, UArg arg1) +{ + volatile uint32_t startTime; + int32_t retVal; + DPU_TrackerProc_OutParams outTrackerProc; + //DPC_TrackerResults trackerResults; + //int32_t errCode; + + while(1) + { + Semaphore_pend(gMmwMssMCB.trackerDPUSemHandle, BIOS_WAIT_FOREVER); + + startTime = Cycleprofiler_getTimeStamp(); + retVal = DPU_TrackerProc_process(gObjDetObj->dpuTrackerObj, gMmwMssMCB.numDetectedPoints, + gMmwMssMCB.pointCloudFromDSP, gMmwMssMCB.pointCloudSideInfoFromDSP, &outTrackerProc); + DebugP_assert(retVal == 0); + + gMmwMssMCB.trackerOutput.currentDescr = outTrackerProc.currentTargetDesc; + gMmwMssMCB.numTargets = outTrackerProc.numTargets[outTrackerProc.currentTargetDesc]; + gMmwMssMCB.trackerOutput.tList[gMmwMssMCB.trackerOutput.currentDescr] = ((trackerProcObjType*)gObjDetObj->dpuTrackerObj)->targetDescrHandle->tList[outTrackerProc.currentTargetDesc]; + gMmwMssMCB.numIndices = outTrackerProc.numIndices[outTrackerProc.currentTargetDesc]; + gMmwMssMCB.trackerOutput.tIndex[gMmwMssMCB.trackerOutput.currentDescr] = ((trackerProcObjType*)gObjDetObj->dpuTrackerObj)->targetDescrHandle->tIndex[outTrackerProc.currentTargetDesc]; + gMmwMssMCB.presenceInd = outTrackerProc.presenceInd[outTrackerProc.currentTargetDesc]; + gMmwMssMCB.trackerProcessingTimeInUsec = (float)(Cycleprofiler_getTimeStamp() - startTime)/(float)R4F_CLOCK_MHZ; + } +} +#endif + +/** + * @b Description + * @n + * DPM Execution Task which executes the DPM Instance which manages the + * HL Profiles executing on the MSS. + * + * @retval + * Not Applicable. + */ +static void mmwDemo_mssDPMTask(UArg arg0, UArg arg1) +{ + int32_t errCode; + DPM_Buffer result; + + while (1) + { + /* Execute the DPM module: */ + errCode = DPM_execute (gMmwMssMCB.objDetDpmHandle, &result); + if (errCode < 0) + { + System_printf ("Error: DPM execution failed [Error code %d]\n", errCode); + } + } +} + + +/************************************************************************** + ******************** Millimeter Wave Demo sensor management Functions ********** + **************************************************************************/ + +/** + * @b Description + * @n + * mmw demo helper Function to do one time sensor initialization. + * User need to fill gMmwMssMCB.cfg.openCfg before calling this function + * + * @param[in] isFirstTimeOpen If true then issues MMwave_open + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t Pcount3DDemo_openSensor(bool isFirstTimeOpen) +{ + int32_t errCode; + MMWave_ErrorLevel errorLevel; + int16_t mmWaveErrorCode; + int16_t subsysErrorCode; + int32_t retVal; + + /* Open mmWave module, this is only done once */ + if (isFirstTimeOpen == true) + { + /*Set LDO bypas configuration */ + System_printf ("Debug: Sending rlRfSetLdoBypassConfig with %d %d %d\n", + gRFLdoBypassCfg.ldoBypassEnable, + gRFLdoBypassCfg.supplyMonIrDrop, + gRFLdoBypassCfg.ioSupplyIndicator); + retVal = rlRfSetLdoBypassConfig(RL_DEVICE_MAP_INTERNAL_BSS, (rlRfLdoBypassCfg_t*)&gRFLdoBypassCfg); + if(retVal != 0) + { + System_printf("Error: rlRfSetLdoBypassConfig retVal=%d\n", retVal); + return -1; + } + + /* Open mmWave module, this is only done once */ + /* Setup the calibration frequency: */ + gMmwMssMCB.cfg.openCfg.freqLimitLow = 600U; + gMmwMssMCB.cfg.openCfg.freqLimitHigh = 640U; + + /* start/stop async events */ + gMmwMssMCB.cfg.openCfg.disableFrameStartAsyncEvent = false; + gMmwMssMCB.cfg.openCfg.disableFrameStopAsyncEvent = false; + + /* No custom calibration: */ + gMmwMssMCB.cfg.openCfg.useCustomCalibration = false; + gMmwMssMCB.cfg.openCfg.customCalibrationEnableMask = 0x0; + + /* calibration monitoring base time unit + * setting it to one frame duration as the demo doesnt support any + * monitoring related functionality + */ + gMmwMssMCB.cfg.openCfg.calibMonTimeUnit = 1; + + + /* Open the mmWave module: */ + if (MMWave_open (gMmwMssMCB.ctrlHandle, &gMmwMssMCB.cfg.openCfg, NULL, &errCode) < 0) + { + /* Error: decode and Report the error */ + MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode); + System_printf ("Error: mmWave Open failed [Error code: %d Subsystem: %d]\n", + mmWaveErrorCode, subsysErrorCode); + return -1; + } +#if 0 // seems to be LVDS related only + /*Set up HSI clock*/ + if(MmwDemo_mssSetHsiClk() < 0) + { + System_printf ("Error: MmwDemo_mssSetHsiClk failed.\n"); + return -1; + } +#endif + /* Open the datapath modules that runs on MSS */ + Pcount3DDemo_dataPathOpen(); + } + return 0; +} + +/** + * @b Description + * @n + * MMW demo helper Function to configure sensor. User need to fill gMmwMssMCB.cfg.ctrlCfg and + * add profiles/chirp to mmWave before calling this function + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t Pcount3DDemo_configSensor(void) +{ + int32_t errCode = 0; + + /* Configure the mmWave module: */ + if (MMWave_config (gMmwMssMCB.ctrlHandle, &gMmwMssMCB.cfg.ctrlCfg, &errCode) < 0) + { + MMWave_ErrorLevel errorLevel; + int16_t mmWaveErrorCode; + int16_t subsysErrorCode; + + /* Error: Report the error */ + MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode); + System_printf ("Error: mmWave Config failed [Error code: %d Subsystem: %d]\n", + mmWaveErrorCode, subsysErrorCode); + } + else + { + errCode = Pcount3DDemo_dataPathConfig(); + if (errCode < 0) + goto exit; +#ifdef TRACKERPROC_EN + errCode = MmwDemo_trackerConfig(); + if (errCode < 0) + goto exit; +#endif + } + +exit: + return errCode; +} + +/** + * @b Description + * @n + * mmw demo helper Function to start sensor. + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t Pcount3DDemo_startSensor(void) +{ + int32_t errCode; + MMWave_CalibrationCfg calibrationCfg; + + /***************************************************************************** + * Data path :: start data path first - this will pend for DPC to ack + *****************************************************************************/ + Pcount3DDemo_dataPathStart(); + + /***************************************************************************** + * RF :: now start the RF and the real time ticking + *****************************************************************************/ + /* Initialize the calibration configuration: */ + memset ((void *)&calibrationCfg, 0, sizeof(MMWave_CalibrationCfg)); + /* Populate the calibration configuration: */ + calibrationCfg.dfeDataOutputMode = gMmwMssMCB.cfg.ctrlCfg.dfeDataOutputMode; + calibrationCfg.u.chirpCalibrationCfg.enableCalibration = false;//true; + calibrationCfg.u.chirpCalibrationCfg.enablePeriodicity = false;//true; + calibrationCfg.u.chirpCalibrationCfg.periodicTimeInFrames = 10U; + + DebugP_log0("App: MMWave_start Issued\n"); + + System_printf("Starting Sensor (issuing MMWave_start)\n"); + + /* Start the mmWave module: The configuration has been applied successfully. */ + if (MMWave_start(gMmwMssMCB.ctrlHandle, &calibrationCfg, &errCode) < 0) + { + MMWave_ErrorLevel errorLevel; + int16_t mmWaveErrorCode; + int16_t subsysErrorCode; + + /* Error/Warning: Unable to stop the mmWave module */ + MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode); + System_printf ("Error: mmWave Start failed [mmWave Error: %d Subsys: %d]\n", mmWaveErrorCode, subsysErrorCode); + return -1; + } + + /***************************************************************************** + * The sensor has been started successfully. Switch on the LED + *****************************************************************************/ + GPIO_write (gMmwMssMCB.cfg.platformCfg.SensorStatusGPIO, 1U); + + gMmwMssMCB.sensorStartCount++; + return 0; +} + +/** + * @b Description + * @n + * Epilog processing after sensor has stopped + * + * @retval None + */ +static void Pcount3DDemo_sensorStopEpilog(void) +{ + Task_Stat stat; + Hwi_StackInfo stackInfo; + Bool stackOverflow; + + /* Print task statistics, note data path has completely stopped due to + * end of frame, so we can do non-real time processing like prints on + * console */ + System_printf("Data Path Stopped (last frame processing done)\n"); + + System_printf("============================================\n"); + System_printf("MSS Task Stack Usage (Note: Task Stack Usage) ==========\n"); + System_printf("%20s %12s %12s %12s\n", "Task Name", "Size", "Used", "Free"); + + + Task_stat(gMmwMssMCB.taskHandles.initTask, &stat); + System_printf("%20s %12d %12d %12d\n", "Init", + stat.stackSize, stat.used, stat.stackSize - stat.used); + + Task_stat(gMmwMssMCB.taskHandles.mmwaveCtrl, &stat); + System_printf("%20s %12d %12d %12d\n", "Mmwave Control", + stat.stackSize, stat.used, stat.stackSize - stat.used); + + Task_stat(gMmwMssMCB.taskHandles.objDetDpmTask, &stat); + System_printf("%20s %12d %12d %12d\n", "ObjDet DPM", + stat.stackSize, stat.used, stat.stackSize - stat.used); + + System_printf("HWI Stack (same as System Stack) Usage ============\n"); + stackOverflow = Hwi_getStackInfo(&stackInfo, TRUE); + if (stackOverflow == TRUE) + { + System_printf("HWI Stack overflowed\n"); + Pcount3DDemo_debugAssert(0); + } + else + { + System_printf("%20s %12s %12s %12s\n", " ", "Size", "Used", "Free"); + System_printf("%20s %12d %12d %12d\n", " ", + stackInfo.hwiStackSize, stackInfo.hwiStackPeak, + stackInfo.hwiStackSize - stackInfo.hwiStackPeak); + } +} + + +/** + * @b Description + * @n + * Stops the RF and datapath for the sensor. Blocks until both operation are completed. + * Prints epilog at the end. + * + * @retval None + */ +void Pcount3DDemo_stopSensor(void) +{ + /* Stop sensor RF , data path will be stopped after RF stop is completed */ + Pcount3DDemo_mmWaveCtrlStop(); + + /* Wait until DPM_stop is completed */ + Semaphore_pend(gMmwMssMCB.DPMstopSemHandle, BIOS_WAIT_FOREVER); + + + + /* Print epilog */ + Pcount3DDemo_sensorStopEpilog(); + + /* The sensor has been stopped successfully. Switch off the LED */ + GPIO_write (gMmwMssMCB.cfg.platformCfg.SensorStatusGPIO, 0U); + + gMmwMssMCB.sensorStopCount++; + + /* print for user */ + System_printf("Sensor has been stopped: startCount: %d stopCount %d\n", + gMmwMssMCB.sensorStartCount,gMmwMssMCB.sensorStopCount); +} + +/** + * @b Description + * @n + * Call back function that was registered during config time and is going + * to be called in DPC processing at the beginning of frame/sub-frame processing, + * we use this to issue BIOS calls for computing CPU load during inter-frame + * + * @param[in] subFrameIndx Sub-frame index of the sub-frame during which processing + * this function was called. + * + * @retval None + */ +static void Pcount3DDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn(uint8_t subFrameIndx) +{ + Load_update(); + gMmwMssMCB.subFrameStats[subFrameIndx].outputStats.interFrameCPULoad = Load_getCPULoad(); +} + +/** + * @b Description + * @n + * Call back function that was registered during config time and is going + * to be called in DPC processing at the beginning of inter-frame/inter-sub-frame processing, + * we use this to issue BIOS calls for computing CPU load during active frame (chirping) + * + * @param[in] subFrameIndx Sub-frame index of the sub-frame during which processing + * this function was called. + * + * @retval None + */ +static void Pcount3DDemo_DPC_ObjectDetection_processInterFrameCallBackFxn(uint8_t subFrameIndx) +{ + Load_update(); + gMmwMssMCB.subFrameStats[subFrameIndx].outputStats.activeFrameCPULoad = Load_getCPULoad(); +} + +/************************************************************************** + ******************** Millimeter Wave Demo init Functions ************************ + **************************************************************************/ + +/** + * @b Description + * @n + * Platform specific hardware initialization. + * + * @param[in] config Platform initialization configuraiton + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_platformInit(Pcount3DDemo_platformCfg *config) +{ + + /* Setup the PINMUX to bring out the UART-1 */ + Pinmux_Set_OverrideCtrl(SOC_XWR68XX_PINN5_PADBE, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL); + Pinmux_Set_FuncSel(SOC_XWR68XX_PINN5_PADBE, SOC_XWR68XX_PINN5_PADBE_MSS_UARTA_TX); + Pinmux_Set_OverrideCtrl(SOC_XWR68XX_PINN4_PADBD, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL); + Pinmux_Set_FuncSel(SOC_XWR68XX_PINN4_PADBD, SOC_XWR68XX_PINN4_PADBD_MSS_UARTA_RX); + + /* Setup the PINMUX to bring out the UART-3 */ + Pinmux_Set_OverrideCtrl(SOC_XWR68XX_PINF14_PADAJ, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL); + Pinmux_Set_FuncSel(SOC_XWR68XX_PINF14_PADAJ, SOC_XWR68XX_PINF14_PADAJ_MSS_UARTB_TX); + + /********************************************************************** + * Setup the PINMUX: + * - GPIO Output: Configure pin K13 as GPIO_2 output + **********************************************************************/ + Pinmux_Set_OverrideCtrl(SOC_XWR68XX_PINK13_PADAZ, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL); + Pinmux_Set_FuncSel(SOC_XWR68XX_PINK13_PADAZ, SOC_XWR68XX_PINK13_PADAZ_GPIO_2); + + /********************************************************************** + * Setup the GPIO: + * - GPIO Output: Configure pin K13 as GPIO_2 output + **********************************************************************/ + config->SensorStatusGPIO = SOC_XWR68XX_GPIO_2; + + /* Initialize the DEMO configuration: */ + config->sysClockFrequency = MSS_SYS_VCLK; + config->loggingBaudRate = 921600; + config->commandBaudRate = 115200; + + /********************************************************************** + * Setup the DS3 LED on the EVM connected to GPIO_2 + **********************************************************************/ + GPIO_setConfig (config->SensorStatusGPIO, GPIO_CFG_OUTPUT); +} + +/** + * @b Description + * @n + * System Initialization Task which initializes the various + * components in the system. + * + * @retval + * Not Applicable. + */ +static void Pcount3DDemo_initTask(UArg arg0, UArg arg1) +{ + int32_t errCode; + MMWave_InitCfg initCfg; + UART_Params uartParams; + Task_Params taskParams; + Semaphore_Params semParams; + DPM_InitCfg dpmInitCfg; + DMA_Params dmaParams; + DMA_Handle dmaHandle; + + DPC_ObjectDetectionRangeHWA_InitParams objDetInitParams; +// int32_t i; + + /* Debug Message: */ + System_printf("Debug: Launched the Initialization Task\n"); + + /***************************************************************************** + * Initialize the mmWave SDK components: + *****************************************************************************/ + /* Initialize the UART */ + UART_init(); + + /* Initialize the DMA for UART */ + DMA_init (); + + /* Open the DMA Instance */ + DMA_Params_init(&dmaParams); + dmaHandle = DMA_open(0, &dmaParams, &errCode); + if (dmaHandle == NULL) + { + printf ("Error: Unable to open the DMA Instance [Error code %d]\n", errCode); + return; + } + + /* Initialize the GPIO */ + GPIO_init(); + + /* Initialize the Mailbox */ + Mailbox_init(MAILBOX_TYPE_MSS); + + /* Platform specific configuration */ + Pcount3DDemo_platformInit(&gMmwMssMCB.cfg.platformCfg); + + /***************************************************************************** + * Open the mmWave SDK components: + *****************************************************************************/ + /* Setup the default UART Parameters */ + UART_Params_init(&uartParams); + uartParams.clockFrequency = gMmwMssMCB.cfg.platformCfg.sysClockFrequency; + uartParams.baudRate = gMmwMssMCB.cfg.platformCfg.commandBaudRate; + uartParams.isPinMuxDone = 1; + + /* Open the UART Instance */ + gMmwMssMCB.commandUartHandle = UART_open(0, &uartParams); + if (gMmwMssMCB.commandUartHandle == NULL) + { + Pcount3DDemo_debugAssert (0); + return; + } + + /* Setup the default UART Parameters */ + UART_Params_init(&uartParams); + uartParams.writeDataMode = UART_DATA_BINARY; + uartParams.readDataMode = UART_DATA_BINARY; + uartParams.clockFrequency = gMmwMssMCB.cfg.platformCfg.sysClockFrequency; + uartParams.baudRate = gMmwMssMCB.cfg.platformCfg.loggingBaudRate; + uartParams.isPinMuxDone = 1U; + + uartParams.dmaHandle = dmaHandle; + uartParams.txDMAChannel = UART_DMA_TX_CHANNEL; + uartParams.rxDMAChannel = UART_DMA_RX_CHANNEL; + + /* Open the Logging UART Instance: */ + gMmwMssMCB.loggingUartHandle = UART_open(1, &uartParams); + if (gMmwMssMCB.loggingUartHandle == NULL) + { + System_printf("Error: Unable to open the Logging UART Instance\n"); + Pcount3DDemo_debugAssert (0); + return; + } + + /* Create binary semaphores which is used to signal DPM_start/DPM_stop/DPM_ioctl is done + * to the sensor management task. The signalling (Semaphore_post) will be done + * from DPM registered report function (which will execute in the DPM execute task context). */ + Semaphore_Params_init(&semParams); + semParams.mode = Semaphore_Mode_BINARY; + gMmwMssMCB.DPMstartSemHandle = Semaphore_create(0, &semParams, NULL); + gMmwMssMCB.DPMstopSemHandle = Semaphore_create(0, &semParams, NULL); + gMmwMssMCB.DPMioctlSemHandle = Semaphore_create(0, &semParams, NULL); + + /* Open EDMA driver */ + Pcount3DDemo_edmaInit(&gMmwMssMCB.dataPathObj, DPC_OBJDET_R4F_EDMA_INSTANCE); + + /* Use EDMA instance 0 on MSS */ + Pcount3DDemo_edmaOpen(&gMmwMssMCB.dataPathObj, DPC_OBJDET_R4F_EDMA_INSTANCE); + + Pcount3DDemo_hwaInit(&gMmwMssMCB.dataPathObj); + Pcount3DDemo_hwaOpen(&gMmwMssMCB.dataPathObj, gMmwMssMCB.socHandle); + + /***************************************************************************** + * mmWave: Initialization of the high level module + *****************************************************************************/ + + /* Initialize the mmWave control init configuration */ + memset ((void*)&initCfg, 0 , sizeof(MMWave_InitCfg)); + + /* Populate the init configuration: */ + initCfg.domain = MMWave_Domain_MSS; + initCfg.socHandle = gMmwMssMCB.socHandle; + initCfg.eventFxn = Pcount3DDemo_eventCallbackFxn; + initCfg.linkCRCCfg.useCRCDriver = 1U; + initCfg.linkCRCCfg.crcChannel = CRC_Channel_CH1; + initCfg.cfgMode = MMWave_ConfigurationMode_FULL; + initCfg.executionMode = MMWave_ExecutionMode_ISOLATION; + + /* Initialize and setup the mmWave Control module */ + gMmwMssMCB.ctrlHandle = MMWave_init (&initCfg, &errCode); + if (gMmwMssMCB.ctrlHandle == NULL) + { + /* Error: Unable to initialize the mmWave control module */ + System_printf ("Error: mmWave Control Initialization failed [Error code %d]\n", errCode); + Pcount3DDemo_debugAssert (0); + return; + } + System_printf ("Debug: mmWave Control Initialization was successful\n"); + + /* Synchronization: This will synchronize the execution of the control module + * between the domains. This is a prerequiste and always needs to be invoked. */ + if (MMWave_sync (gMmwMssMCB.ctrlHandle, &errCode) < 0) + { + /* Error: Unable to synchronize the mmWave control module */ + System_printf ("Error: mmWave Control Synchronization failed [Error code %d]\n", errCode); + Pcount3DDemo_debugAssert (0); + return; + } + System_printf ("Debug: mmWave Control Synchronization was successful\n"); + + /***************************************************************************** + * Launch the mmWave control execution task + * - This should have a higher priroity than any other task which uses the + * mmWave control API + *****************************************************************************/ + Task_Params_init(&taskParams); + taskParams.priority = MMWDEMO_MMWAVE_CTRL_TASK_PRIORITY; + taskParams.stackSize = 2800; + gMmwMssMCB.taskHandles.mmwaveCtrl = Task_create(Pcount3DDemo_mmWaveCtrlTask, &taskParams, NULL); + + + /***************************************************************************** + * Create a task to do DMA based UART data transfer + *****************************************************************************/ + /* Create a binary semaphore for application task to pend */ + Semaphore_Params_init(&semParams); + semParams.mode = Semaphore_Mode_BINARY; + gMmwMssMCB.uartTxSemHandle = Semaphore_create(0, &semParams, NULL); + + Task_Params_init(&taskParams); + taskParams.priority = MMWDEMO_UARTTX_TASK_PRIORITY; + taskParams.stackSize = 800; + Task_create(MmwDemo_uartTxTask, &taskParams, NULL); + + /***************************************************************************** + * Initialization of the DPM Module: + *****************************************************************************/ + memset ((void *)&objDetInitParams, 0, sizeof(DPC_ObjectDetectionRangeHWA_InitParams)); + + /* Note this must be after Pcount3DDemo_dataPathOpen() above which opens the hwa */ + objDetInitParams.L3ramCfg.addr = (void *)&gMmwL3[0]; + objDetInitParams.L3ramCfg.size = sizeof(gMmwL3); + objDetInitParams.CoreLocalRamCfg.addr = &gDPCTCM[0]; + objDetInitParams.CoreLocalRamCfg.size = sizeof(gDPCTCM); + objDetInitParams.edmaHandle = gMmwMssMCB.dataPathObj.edmaHandle; + objDetInitParams.hwaHandle = gMmwMssMCB.dataPathObj.hwaHandle; + + /* DPC Call-back config */ + objDetInitParams.processCallBackFxn.processInterFrameBeginCallBackFxn = + Pcount3DDemo_DPC_ObjectDetection_processInterFrameCallBackFxn; + + objDetInitParams.processCallBackFxn.processFrameBeginCallBackFxn = + Pcount3DDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn; + + /* Setup the configuration: */ + memset ((void *)&dpmInitCfg, 0, sizeof(DPM_InitCfg)); + dpmInitCfg.socHandle = gMmwMssMCB.socHandle; + dpmInitCfg.ptrProcChainCfg = &gDPC_ObjDetRangeHWACfg;; + dpmInitCfg.instanceId = DPC_OBJDET_R4F_INSTANCEID; + dpmInitCfg.domain = DPM_Domain_DISTRIBUTED; + dpmInitCfg.reportFxn = Pcount3DDemo_DPC_ObjectDetection_reportFxn; + dpmInitCfg.arg = &objDetInitParams; + dpmInitCfg.argSize = sizeof(DPC_ObjectDetectionRangeHWA_InitParams); + + /* Initialize the DPM Module: */ + gMmwMssMCB.objDetDpmHandle = DPM_init (&dpmInitCfg, &errCode); + if (gMmwMssMCB.objDetDpmHandle == NULL) + { + System_printf ("Error: Unable to initialize the DPM Module [Error: %d]\n", errCode); + Pcount3DDemo_debugAssert (0); + return; + } + + /* Synchronization: This will synchronize the execution of the datapath module + * between the domains. This is a prerequiste and always needs to be invoked. */ + while (1) + { + int32_t syncStatus; + + /* Get the synchronization status: */ + syncStatus = DPM_synch (gMmwMssMCB.objDetDpmHandle, &errCode); + if (syncStatus < 0) + { + /* Error: Unable to synchronize the framework */ + System_printf ("Error: DPM Synchronization failed [Error code %d]\n", errCode); + Pcount3DDemo_debugAssert (0); + return; + } + if (syncStatus == 1) + { + /* Synchronization acheived: */ + break; + } + /* Sleep and poll again: */ + Task_sleep(1); + } + +#ifdef TRACKERPROC_EN + /***************************************************************************** + * Create a task to run tracker DPU at lower priority than HWA DPC + *****************************************************************************/ + /* Create a binary semaphore for application task to pend */ + Semaphore_Params_init(&semParams); + semParams.mode = Semaphore_Mode_BINARY; + gMmwMssMCB.trackerDPUSemHandle = Semaphore_create(0, &semParams, NULL); + + Task_Params_init(&taskParams); + taskParams.priority = MMWDEMO_TRACKERDPU_TASK_PRIORITY; + taskParams.stackSize = 7*1024; + Task_create(MmwDemo_trackerDPUTask, &taskParams, NULL); +#endif + + /* Launch the DPM Task */ + Task_Params_init(&taskParams); + taskParams.priority = MMWDEMO_DPC_OBJDET_DPM_TASK_PRIORITY; + taskParams.stackSize = 7*1024; + gMmwMssMCB.taskHandles.objDetDpmTask = Task_create(mmwDemo_mssDPMTask, &taskParams, NULL); + + /***************************************************************************** + * Initialize the Profiler + *****************************************************************************/ + Cycleprofiler_init(); + + /***************************************************************************** + * Initialize the CLI Module: + *****************************************************************************/ + Pcount3DDemo_CLIInit(MMWDEMO_CLI_TASK_PRIORITY); + + return; +} + +/** + * @b Description + * @n + * Function to sleep the R4F using WFI (Wait For Interrupt) instruction. + * When R4F has no work left to do, + * the BIOS will be in Idle thread and will call this function. The R4F will + * wake-up on any interrupt (e.g chirp interrupt). + * + * @retval + * Not Applicable. + */ +void Pcount3DDemo_sleep(void) +{ + /* issue WFI (Wait For Interrupt) instruction */ + asm(" WFI "); +} + +/** + * @b Description + * @n + * Entry point into the Millimeter Wave Demo + * + * @retval + * Not Applicable. + */ +int main (void) +{ + Task_Params taskParams; + int32_t errCode; + SOC_Handle socHandle; + SOC_Cfg socCfg; + + /* Initialize the ESM: Dont clear errors as TI RTOS does it */ + ESM_init(0U); + + /* Initialize the SOC confiugration: */ + memset ((void *)&socCfg, 0, sizeof(SOC_Cfg)); + + /* Populate the SOC configuration: */ + socCfg.clockCfg = SOC_SysClock_INIT; + socCfg.mpuCfg = SOC_MPUCfg_CONFIG; + socCfg.dssCfg = SOC_DSSCfg_UNHALT; + + /* Initialize the SOC Module: This is done as soon as the application is started + * to ensure that the MPU is correctly configured. */ + socHandle = SOC_init (&socCfg, &errCode); + if (socHandle == NULL) + { + System_printf ("Error: SOC Module Initialization failed [Error code %d]\n", errCode); + Pcount3DDemo_debugAssert (0); + return -1; + } + + /* Initialize and populate the demo MCB */ + memset ((void*)&gMmwMssMCB, 0, sizeof(Pcount3DDemo_MSS_MCB)); + + gMmwMssMCB.socHandle = socHandle; + + /* Debug Message: */ + System_printf ("**********************************************\n"); + System_printf ("Debug: Launching the MMW Demo on MSS\n"); + System_printf ("**********************************************\n"); + + /* Initialize the Task Parameters. */ + Task_Params_init(&taskParams); + gMmwMssMCB.taskHandles.initTask = Task_create(Pcount3DDemo_initTask, &taskParams, NULL); + + /* Start BIOS */ + BIOS_start(); + return 0; +} + diff --git a/mss/pcount3D_cli.c b/mss/pcount3D_cli.c new file mode 100644 index 0000000..13c5efa --- /dev/null +++ b/mss/pcount3D_cli.c @@ -0,0 +1,1195 @@ +/* + * @file mmw_cli.c + * + * @brief + * Mmw (Milli-meter wave) DEMO CLI Implementation + * + * \par + * NOTE: + * (C) Copyright 2018 Texas Instruments, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/************************************************************************** + *************************** Include Files ******************************** + **************************************************************************/ + +/* Standard Include Files. */ +#include +#include +#include +#include +#include + +/* BIOS/XDC Include Files. */ +#include + +/* mmWave SDK Include Files: */ +#include +#include +#include +#include +#include +#include + +/* Demo Include Files */ +#include +#include + +#include +#include +#include + +#define DEBUG(_x) //_x + +/************************************************************************** + *************************** Local function prototype**************************** + **************************************************************************/ + +/* CLI Extended Command Functions */ +static int32_t Pcount3DDemo_CLIDynRACfarCfg (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIStaticRACfarCfg (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIDynRngAngleCfg (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIStaticRngAngleCfg (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIDynAngleEstCfg (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIDopplerCFARCfg (int32_t argc, char* argv[]); + +static int32_t Pcount3DDemo_CLIBoardAntGeometry0 (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIBoardAntGeometry1 (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIBoardAntPhaseRot (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIAntAngleFoV (int32_t argc, char* argv[]); + +static int32_t Pcount3DDemo_CLISensorStart (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLISensorStop (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLIADCBufCfg (int32_t argc, char* argv[]); +static int32_t Pcount3DDemo_CLICompRangeBiasAndRxChanPhaseCfg (int32_t argc, char* argv[]); + +/************************************************************************** + *************************** Extern Definitions ******************************* + **************************************************************************/ + +extern Pcount3DDemo_MSS_MCB gMmwMssMCB; + +/************************************************************************** + *************************** Local Definitions **************************** + **************************************************************************/ + + +/************************************************************************** + *************************** CLI Function Definitions ************************** + **************************************************************************/ +/** + * @b Description + * @n + * This is the CLI Handler for the sensor start command + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLISensorStart (int32_t argc, char* argv[]) +{ + bool doReconfig = true; + int32_t retVal = 0; + + /* Only following command syntax will be supported + sensorStart + sensorStart 0 + */ + if (argc == 2) + { + doReconfig = (bool) atoi (argv[1]); + + if (doReconfig == true) + { + CLI_write ("Error: Reconfig is not supported, only argument of 0 is\n" + "(do not reconfig, just re-start the sensor) valid\n"); + return -1; + } + } + else + { + /* In case there is no argument for sensorStart, always do reconfig */ + doReconfig = true; + } + + /*********************************************************************************** + * Do sensor state management to influence the sensor actions + ***********************************************************************************/ + + /* Error checking initial state */ + if (gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_INIT) + { + MMWave_CtrlCfg ctrlCfg; + + /* need to get number of sub-frames so that next function to check + * pending state can work */ + CLI_getMMWaveExtensionConfig (&ctrlCfg); + gMmwMssMCB.objDetCommonCfg.numSubFrames = + MmwDemo_RFParser_getNumSubFrames(&ctrlCfg); + + if (Pcount3DDemo_isAllCfgInPendingState() == 0) + { + CLI_write ("Error: Full configuration must be provided before sensor can be started " + "the first time\n"); + + /* Although not strictly needed, bring back to the initial value since we + * are rejecting this first time configuration, prevents misleading debug. */ + gMmwMssMCB.objDetCommonCfg.numSubFrames = 0; + + return -1; + } + } + + if (gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_STARTED) + { + CLI_write ("Ignored: Sensor is already started\n"); + return 0; + } + + if (doReconfig == false) + { + /* User intends to issue sensor start without config, check if no + config was issued after stop and generate error if this is the case. */ + if (Pcount3DDemo_isAllCfgInNonPendingState() == 0) + { + /* Message user differently if all config was issued or partial config was + issued. */ + if (Pcount3DDemo_isAllCfgInPendingState()) + { + CLI_write ("Error: You have provided complete new configuration, " + "issue \"sensorStart\" (without argument) if you want it to " + "take effect\n"); + } + else + { + CLI_write ("Error: You have provided partial configuration between stop and this " + "command and partial configuration cannot be undone." + "Issue the full configuration and do \"sensorStart\" \n"); + } + return -1; + } + } + else + { + /* User intends to issue sensor start with full config, check if all config + was issued after stop and generate error if is the case. */ + if (Pcount3DDemo_isAllCfgInPendingState() == 0) + { + /* Message user differently if no config was issued or partial config was + issued. */ + if (Pcount3DDemo_isAllCfgInNonPendingState()) + { + CLI_write ("Error: You have provided no configuration, " + "issue \"sensorStart 0\" OR provide " + "full configuration and issue \"sensorStart\"\n"); + } + else + { + CLI_write ("Error: You have provided partial configuration between stop and this " + "command and partial configuration cannot be undone." + "Issue the full configuration and do \"sensorStart\" \n"); + } + return -1; + } + } + + /*********************************************************************************** + * Retreive and check mmwave Open related config before calling openSensor + ***********************************************************************************/ + + /* Fill demo's MCB mmWave openCfg structure from the CLI configs*/ + if (gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_INIT) + { + /* Get the open configuration: */ + CLI_getMMWaveExtensionOpenConfig (&gMmwMssMCB.cfg.openCfg); + } + else + { + /* openCfg related configurations like chCfg, lowPowerMode, adcCfg + * are only used on the first sensor start. If they are different + * on a subsequent sensor start, then generate a fatal error + * so the user does not think that the new (changed) configuration + * takes effect, the board needs to be reboot for the new + * configuration to be applied. + */ + MMWave_OpenCfg openCfg; + CLI_getMMWaveExtensionOpenConfig (&openCfg); + /* Compare openCfg->chCfg*/ + if(memcmp((void *)&gMmwMssMCB.cfg.openCfg.chCfg, (void *)&openCfg.chCfg, + sizeof(rlChanCfg_t)) != 0) + { + Pcount3DDemo_debugAssert(0); + } + + /* Compare openCfg->lowPowerMode*/ + if(memcmp((void *)&gMmwMssMCB.cfg.openCfg.lowPowerMode, (void *)&openCfg.lowPowerMode, + sizeof(rlLowPowerModeCfg_t)) != 0) + { + Pcount3DDemo_debugAssert(0); + } + /* Compare openCfg->adcOutCfg*/ + if(memcmp((void *)&gMmwMssMCB.cfg.openCfg.adcOutCfg, (void *)&openCfg.adcOutCfg, + sizeof(rlAdcOutCfg_t)) != 0) + { + Pcount3DDemo_debugAssert(0); + } + } + + retVal = Pcount3DDemo_openSensor(gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_INIT); + if(retVal != 0) + { + return -1; + } + + /*********************************************************************************** + * Retrieve mmwave Control related config before calling startSensor + ***********************************************************************************/ + /* Get the mmWave ctrlCfg from the CLI mmWave Extension */ + if(doReconfig) + { + CLI_getMMWaveExtensionConfig (&gMmwMssMCB.cfg.ctrlCfg); + retVal = Pcount3DDemo_configSensor(); + if(retVal != 0) + { + return -1; + } + } + retVal = Pcount3DDemo_startSensor(); + if(retVal != 0) + { + return -1; + } + + /*********************************************************************************** + * Set the state + ***********************************************************************************/ + gMmwMssMCB.sensorState = Pcount3DDemo_SensorState_STARTED; + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for the sensor stop command + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLISensorStop (int32_t argc, char* argv[]) +{ + if ((gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_STOPPED) || + (gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_INIT)) + { + CLI_write ("Ignored: Sensor is already stopped\n"); + return 0; + } + + Pcount3DDemo_stopSensor(); + + Pcount3DDemo_resetStaticCfgPendingState(); + + gMmwMssMCB.sensorState = Pcount3DDemo_SensorState_STOPPED; + return 0; +} + +/** + * @b Description + * @n + * Utility function to get sub-frame number + * + * @param[in] argc Number of arguments + * @param[in] argv Arguments + * @param[in] expectedArgc Expected number of arguments + * @param[out] subFrameNum Sub-frame Number (0 based) + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIGetSubframe (int32_t argc, char* argv[], int32_t expectedArgc, + int8_t* subFrameNum) +{ + int8_t subframe; + + /* Sanity Check: Minimum argument check */ + if (argc != expectedArgc) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + /*Subframe info is always in position 1*/ + subframe = (int8_t) atoi(argv[1]); + + if(subframe >= (int8_t)RL_MAX_SUBFRAMES) + { + CLI_write ("Error: Subframe number is invalid\n"); + return -1; + } + + *subFrameNum = (int8_t)subframe; + + return 0; +} + + + +/** + * @b Description + * @n + * This is the CLI Handler for dynamic scene RA CFAR configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIDynRACfarCfg (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 15, &subFrameNum) < 0) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIDynRACfarCfg argc = %d, argv2 = %d\n", argc, (uint8_t) atoi (argv[2]));) + return -1; + } + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.leftSkipSize = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.rightSkipSize = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.leftSkipSizeAzimuth = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.rightSkipSizeAzimuth= (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.searchWinSizeRange = (uint8_t) atoi (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.searchWinSizeDoppler= (uint8_t) atoi (argv[7]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.guardSizeRange = (uint8_t) atoi (argv[8]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.guardSizeDoppler = (uint8_t) atoi (argv[9]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.K0 = (float) atof (argv[10]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.dopplerSearchRelThr = (float) atof (argv[11]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.enableSecondPassSearch = (uint8_t) atoi (argv[13]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicSideLobeThr = (float) atof (argv[12]); + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.leftSkipSize = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.rightSkipSize = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.leftSkipSizeAzimuth = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.rightSkipSizeAzimuth= (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.searchWinSizeRange = (uint8_t) atoi (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.searchWinSizeDoppler= (uint8_t) atoi (argv[7]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.guardSizeRange = (uint8_t) atoi (argv[8]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.guardSizeDoppler = (uint8_t) atoi (argv[9]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.K0 = (float) atof (argv[10]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.dopplerSearchRelThr = (float) atof (argv[11]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicCfarConfig.enableSecondPassSearch = (uint8_t) atoi (argv[13]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.dynamicSideLobeThr = (float) atof (argv[12]); + } + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for static scene RA CFAR configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIStaticRACfarCfg (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 15, &subFrameNum) < 0) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIStaticRACfarCfg argc = %d, argv2 = %d\n", argc, (uint8_t) atoi (argv[2]));) + return -1; + } + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.leftSkipSize = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.rightSkipSize = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.leftSkipSizeAzimuth = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.rightSkipSizeAzimuth= (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.searchWinSizeRange = (uint8_t) atoi (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.searchWinSizeDoppler= (uint8_t) atoi (argv[7]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.guardSizeRange = (uint8_t) atoi (argv[8]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.guardSizeDoppler = (uint8_t) atoi (argv[9]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.K0 = (float) atof (argv[10]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.dopplerSearchRelThr = (float) atof (argv[11]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.enableSecondPassSearch = (uint8_t) atoi (argv[13]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticSideLobeThr = (float) atof (argv[12]); + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.leftSkipSize = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.rightSkipSize = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.leftSkipSizeAzimuth = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.rightSkipSizeAzimuth= (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.searchWinSizeRange = (uint8_t) atoi (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.searchWinSizeDoppler= (uint8_t) atoi (argv[7]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.guardSizeRange = (uint8_t) atoi (argv[8]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.guardSizeDoppler = (uint8_t) atoi (argv[9]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.K0 = (float) atof (argv[10]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.dopplerSearchRelThr = (float) atof (argv[11]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticCfarConfig.enableSecondPassSearch = (uint8_t) atoi (argv[13]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.staticSideLobeThr = (float) atof (argv[12]); + } + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for dynamic scene range-angle config + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIDynRngAngleCfg (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 6, &subFrameNum) < 0) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIDynRngAngleCfg argc = %d, argv2 = %f\n", argc, (float) atof (argv[2]));) + return -1; + } + + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.searchStep = (float) atof (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.mvdr_alpha = (float) atof (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.detectionMethod = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.dopplerEstMethod = (uint8_t) atoi (argv[5]); + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.searchStep = (float) atof (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.mvdr_alpha = (float) atof (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.detectionMethod = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.dopplerEstMethod = (uint8_t) atoi (argv[5]); + } + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for dynamic scene 2D angle estimation config + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIDynAngleEstCfg (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + subFrameNum = (int8_t) atoi(argv[1]); + + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + DEBUG(System_printf ("Error: Pcount3DDemo_CLIDynAngleEstCfg all detectionMethod = %d\n", gMmwMssMCB.subFrameCfg[0].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.detectionMethod);) + if (gMmwMssMCB.subFrameCfg[0].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.detectionMethod <= 1) + { + if (argc != 10) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIDynAngleEstCfg argc = %d, argv2 = %f\n", argc, (float) atof (argv[2]));) + return -1; + } + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.elevSearchStep = (float) atof (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.mvdr_alpha = (float) atof (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.maxNpeak2Search = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.peakExpSamples = (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.elevOnly = (uint8_t) atoi (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.sideLobThr = (float) atof (argv[7]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.peakExpRelThr = (float) atof (argv[8]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.peakExpSNRThr = (float) atof (argv[9]); + } + } + else + { + if (argc != 8) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIDynAngleEstCfg argc = %d, argv2 = %f\n", argc, (float) atof (argv[2]));) + return -1; + } + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.zoominFactor = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.zoominNn8bors = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.peakExpSamples = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.peakExpRelThr = (float) atof (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.peakExpSNRThr = (float) atof (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.localMaxCheckFlag = (uint8_t) atoi(argv[7]); + } + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + if (gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.detectionMethod <= 1) + { + if (argc != 10) + { + return -1; + } + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.elevSearchStep = (float) atof (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.mvdr_alpha = (float) atof (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.maxNpeak2Search = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.peakExpSamples = (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.elevOnly = (uint8_t) atoi (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.sideLobThr = (float) atof (argv[7]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.peakExpRelThr = (float) atof (argv[8]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevAngleEstCfg.peakExpSNRThr = (float) atof (argv[9]); + } + else + { + if (argc != 8) + { + return -1; + } + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.zoominFactor = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.zoominNn8bors = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.peakExpSamples = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.peakExpRelThr = (float) atof (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.peakExpSNRThr = (float) atof (argv[6]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.angle2DEst.azimElevZoominCfg.localMaxCheckFlag = (uint8_t) atoi(argv[7]); + + } + } + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for Doppler Estimation configuration if the Doppler estimation method is CFAR + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIDopplerCFARCfg (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 7, &subFrameNum) < 0) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIDopplerCFARCfg argc = %d, argv2\n", argc, (uint8_t) atoi (argv[2]));) + return -1; + } + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + if (gMmwMssMCB.subFrameCfg[0].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.dopplerEstMethod == 1) + { + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.cfarDiscardLeft = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.cfarDiscardRight = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.guardWinSize = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.refWinSize = (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.thre = (float) atof (argv[6]); + } + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + if (gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.rangeAngleCfg.dopplerEstMethod == 1) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.cfarDiscardLeft = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.cfarDiscardRight = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.guardWinSize = (uint8_t) atoi (argv[4]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.refWinSize = (uint8_t) atoi (argv[5]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.dopCfarCfg.thre = (float) atof (argv[6]); + } + } + return 0; +} + + +/** + * @b Description + * @n + * This is the CLI Handler for static scene range-angle config + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIStaticRngAngleCfg (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 5, &subFrameNum) < 0) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIStaticRngAngleCfg argc = %d, argv2 = %d\n", argc, (uint8_t) atoi (argv[2]));) + return -1; + } + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.staticEstCfg.staticProcEnabled = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.staticEstCfg.staticAzimStepDeciFactor = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.staticEstCfg.staticElevStepDeciFactor = (uint8_t) atoi (argv[4]); + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.staticEstCfg.staticProcEnabled = (uint8_t) atoi (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.staticEstCfg.staticAzimStepDeciFactor = (uint8_t) atoi (argv[3]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.staticEstCfg.staticElevStepDeciFactor = (uint8_t) atoi (argv[4]); + } + return 0; +} + + + +/** + * @b Description + * @n + * This is the CLI Handler for ADC buffer command + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIADCBufCfg (int32_t argc, char* argv[]) +{ + MmwDemo_ADCBufCfg adcBufCfg; + int8_t subFrameNum; + + if (gMmwMssMCB.sensorState == Pcount3DDemo_SensorState_STARTED) + { + CLI_write ("Ignored: This command is not allowed after sensor has started\n"); + return 0; + } + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 6, &subFrameNum) < 0) + { + return -1; + } + + /* Initialize the ADC Output configuration: */ + memset ((void *)&adcBufCfg, 0, sizeof(adcBufCfg)); + + /* Populate configuration: */ + adcBufCfg.adcFmt = (uint8_t) atoi (argv[2]); + adcBufCfg.iqSwapSel = (uint8_t) atoi (argv[3]); + adcBufCfg.chInterleave = (uint8_t) atoi (argv[4]); + adcBufCfg.chirpThreshold = (uint8_t) atoi (argv[5]); + + /* This demo is using HWA for 1D processing which does not allow multi-chirp + * processing */ + if (adcBufCfg.chirpThreshold != 1) + { + CLI_write("Error: chirpThreshold must be 1, multi-chirp is not allowed\n"); + return -1; + } + /* Save Configuration to use later */ + Pcount3DDemo_CfgUpdate((void *)&adcBufCfg, + PCOUNT3DDEMO_ADCBUFCFG_OFFSET, + sizeof(MmwDemo_ADCBufCfg), subFrameNum); + return 0; +} + + +/** + * @b Description + * @n + * This is the CLI Handler for compensation of range bias and channel phase offsets + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLICompRangeBiasAndRxChanPhaseCfg (int32_t argc, char* argv[]) +{ + int32_t argInd; + int32_t i; + uint8_t indx; + + /* Sanity Check: Minimum argument check */ + if (argc != (1+1+SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL*2)) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + DEBUG(System_printf ("Error: Pcount3DDemo_CLICompRangeBiasAndRxChanPhaseCfg argc = %d, argv2=%f\n", argc, (float)atof (argv[2]));) + return -1; + } + + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + argInd = 2; + for (i=0; i < SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL; i++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.phaseCompVect[i].real = (float)atof (argv[argInd++]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.phaseCompVect[i].imag = (float)atof (argv[argInd++]); + } + } + + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for board antenna geometry matrix row 0 + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIBoardAntGeometry0 (int32_t argc, char* argv[]) +{ + int32_t argInd; + int32_t i; + uint8_t indx; + + /* Sanity Check: Minimum argument check */ + if (argc != (1+SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL)) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + DEBUG(System_printf ("Error: Pcount3DDemo_CLIBoardAntGeometry0 argc = %d, argv2 = %d\n", argc, (int8_t)atoi (argv[2]));) + return -1; + } + + + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + argInd = 1; + for (i=0; i < SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL; i++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.m_ind[i] = (int8_t)atoi (argv[argInd++]); + } + } + + return 0; +} + + +/** + * @b Description + * @n + * This is the CLI Handler for board antenna geometry matrix row 1 + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIBoardAntGeometry1 (int32_t argc, char* argv[]) +{ + int32_t argInd; + int32_t i; + uint8_t indx; + + /* Sanity Check: Minimum argument check */ + if (argc != (1+SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL)) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + DEBUG(System_printf ("Error: Pcount3DDemo_CLIBoardAntGeometry1 argc = %d, argv2 = %d\n", argc, (int8_t)atoi (argv[2]));) + return -1; + } + + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + argInd = 1; + for (i=0; i < SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL; i++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.n_ind[i] = (int8_t)atoi (argv[argInd++]); + } + } + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for board antenna phase rotation + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIBoardAntPhaseRot (int32_t argc, char* argv[]) +{ + int32_t argInd; + int32_t i; + uint8_t indx; + + /* Sanity Check: Minimum argument check */ + if (argc != (1+SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL)) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + DEBUG(System_printf ("Error: Pcount3DDemo_CLIBoardAntPhaseRot argc = %d, argv2 = %d\n", argc, (int8_t)atoi (argv[2]));) + return -1; + } + + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + argInd = 1; + for (i=0; i < SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL; i++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.phaseRot[i] = (int8_t)atoi (argv[argInd++]); + } + } + return 0; +} + +/** + * @b Description + * @n + * This is the CLI Handler for angle FOV config + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t Pcount3DDemo_CLIAntAngleFoV (int32_t argc, char* argv[]) +{ + int8_t subFrameNum; + + if(Pcount3DDemo_CLIGetSubframe(argc, argv, 4, &subFrameNum) < 0) + { + DEBUG(System_printf ("Error: Pcount3DDemo_CLIAntAngleFoV argc = %d, argv2 = %f\n", argc, (float) atof (argv[2]));) + return -1; + } + + if(subFrameNum == PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG) + { + uint8_t indx; + for(indx = 0; indx < RL_MAX_SUBFRAMES; indx++) + { + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.fovCfg[0] = (float) atof (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.fovCfg[1] = (float) atof (argv[3]); + } + } + else + { + uint8_t indx = subFrameNum; + /* Apply configuration to specific subframe (or to position zero for the legacy case + where there is no advanced frame config) */ + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.fovCfg[0] = (float) atof (argv[2]); + gMmwMssMCB.subFrameCfg[indx].objDetDynCfg.dspDynCfg.caponChainCfg.doaConfig.fovCfg[1] = (float) atof (argv[3]); + } + return 0; +} + + + +/** + * @b Description + * @n + * This is the CLI Execution Task + * + * @retval + * Not Applicable. + */ +void Pcount3DDemo_CLIInit (uint8_t taskPriority) +{ + CLI_Cfg cliCfg; + char demoBanner[110]; + uint32_t cnt; + + /* Create Demo Banner to be printed out by CLI */ + sprintf(&demoBanner[0], + "***********************************\n" \ + "IWR68xx Indoor people counting demo" \ + "***********************************\n" + ); + + /* Initialize the CLI configuration: */ + memset ((void *)&cliCfg, 0, sizeof(CLI_Cfg)); + + /* Populate the CLI configuration: */ + cliCfg.cliPrompt = "mmwDemo:/>"; + cliCfg.cliBanner = demoBanner; + cliCfg.cliUartHandle = gMmwMssMCB.commandUartHandle; + cliCfg.taskPriority = taskPriority; + cliCfg.socHandle = gMmwMssMCB.socHandle; + cliCfg.mmWaveHandle = gMmwMssMCB.ctrlHandle; + cliCfg.enableMMWaveExtension = 1U; + cliCfg.usePolledMode = true; + cliCfg.overridePlatform = false; + cliCfg.overridePlatformString = NULL; + + cnt=0; + cliCfg.tableEntry[cnt].cmd = "sensorStart"; + cliCfg.tableEntry[cnt].helpString = "[doReconfig(optional, default:enabled)]"; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLISensorStart; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "sensorStop"; + cliCfg.tableEntry[cnt].helpString = "No arguments"; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLISensorStop; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "dynamicRACfarCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIDynRACfarCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "staticRACfarCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIStaticRACfarCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "dynamicRangeAngleCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIDynRngAngleCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "dynamic2DAngleCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIDynAngleEstCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "dopplerCfarCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIDopplerCFARCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "staticRangeAngleCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIStaticRngAngleCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "fovCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIAntAngleFoV; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "antGeometry0"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIBoardAntGeometry0; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "antGeometry1"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIBoardAntGeometry1; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "antPhaseRot"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIBoardAntPhaseRot; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "adcbufCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLIADCBufCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "compRangeBiasAndRxChanPhase"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = Pcount3DDemo_CLICompRangeBiasAndRxChanPhaseCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "trackingCfg"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLITrackingCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "staticBoundaryBox"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLIStaticBoundaryBoxCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "boundaryBox"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLIBoundaryBoxCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "sensorPosition"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLISensorPositionCfg; + cnt++; + cliCfg.tableEntry[cnt].cmd = "gatingParam";// PC: 4 gating volume, Limits are set to 3m in length, 2m in width, 0 no limit in doppler + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLIGatingParamCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "stateParam";// PC: 10 frames to activate, 5 to forget, 10 active to free, 1000 static to free, 5 exit to free, 6000 sleep to free + cliCfg.tableEntry[cnt].helpString = " ";//det2act, det2free, act2free, stat2free, exit2free, sleep2free + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLIStateParamCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "allocationParam";// PC: 250 SNR, 0.1 minimal velocity, 5 points, 1m in distance, 2m/s in velocity + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLIAllocationParamCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "maxAcceleration"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemoCLIMaxAccelerationParamCfg; + cnt++; + + cliCfg.tableEntry[cnt].cmd = "presenceBoundaryBox"; + cliCfg.tableEntry[cnt].helpString = " "; + cliCfg.tableEntry[cnt].cmdHandlerFxn = MmwDemo_CLIPresenceParamCfg; + cnt++; + + /* Open the CLI: */ + if (CLI_open (&cliCfg) < 0) + { + System_printf ("Error: Unable to open the CLI\n"); + return; + } + System_printf ("Debug: CLI is operational\n"); + return; +} + + diff --git a/mss/pcount3D_mss.cfg b/mss/pcount3D_mss.cfg new file mode 100644 index 0000000..0efe5e6 --- /dev/null +++ b/mss/pcount3D_mss.cfg @@ -0,0 +1,90 @@ +/* + * Copyright 2011 by Texas Instruments Incorporated. + * + * All rights reserved. Property of Texas Instruments Incorporated. + * Restricted rights to use, duplicate or disclose this code are + * granted through contract. + * + */ +environment['xdc.cfg.check.fatal'] = 'false'; + +/******************************************************************** + ************************** BIOS Modules **************************** + ********************************************************************/ +var Memory = xdc.useModule('xdc.runtime.Memory'); +var BIOS = xdc.useModule('ti.sysbios.BIOS'); +var HeapMem = xdc.useModule('ti.sysbios.heaps.HeapMem'); +var HeapBuf = xdc.useModule('ti.sysbios.heaps.HeapBuf'); +var Task = xdc.useModule('ti.sysbios.knl.Task'); +var Idle = xdc.useModule('ti.sysbios.knl.Idle'); +var SEM = xdc.useModule('ti.sysbios.knl.Semaphore'); +var Event = xdc.useModule('ti.sysbios.knl.Event'); +var Hwi = xdc.useModule('ti.sysbios.family.arm.v7r.vim.Hwi'); +var System = xdc.useModule('xdc.runtime.System'); +var SysStd = xdc.useModule('xdc.runtime.SysStd'); +var clock = xdc.useModule('ti.sysbios.knl.Clock'); +var Pmu = xdc.useModule('ti.sysbios.family.arm.v7a.Pmu'); +var Load = xdc.useModule('ti.sysbios.utils.Load'); + +System.SupportProxy = SysStd; + +/* FIQ Stack Usage: */ +Hwi.fiqStackSize = 2048; +Hwi.fiqStackSection = ".myFiqStack" +Program.sectMap[".myFiqStack"] = "DATA_RAM"; + +/* Default Heap Creation: Local L2 memory */ +var heapMemParams = new HeapMem.Params(); +heapMemParams.size = 130*1024; +heapMemParams.sectionName = "systemHeap"; +Program.global.heap0 = HeapMem.create(heapMemParams); +Memory.defaultHeapInstance = Program.global.heap0; +System.extendedFormats = '%$L%$S%$F%f'; + +/* Enable BIOS Task Scheduler */ +BIOS.taskEnabled = true; + +/* Reduce the size of BIOS */ +BIOS.swiEnabled = false; /* We don't use SWIs */ +BIOS.libType = BIOS.LibType_Custom; +Program.stack = 2048; /* for isr context */ +Task.idleTaskStackSize = 800; +var Text = xdc.useModule('xdc.runtime.Text'); +Text.isLoaded = false; + +/* do not call update for load - Application will call it at inter-frame boundary */ +Load.updateInIdle = false; + +/* Install idle function to sleep the R4F (using WFI instruction). Note above + Load.updateInIdle is false which allows to sleep the R4F in idle. + Also, no other book-keeping etc functions should be installed in the idle thread */ +Idle.addFunc('&Pcount3DDemo_sleep'); + +Program.sectMap[".vecs"] = "VECTORS"; + +/* Make sure libraries are built with 32-bit enum types to be compatible with DSP enum types*/ +BIOS.includeXdcRuntime = true; +BIOS.libType = BIOS.LibType_Custom; +BIOS.customCCOpts += " --enum_type=int "; + +/******************************************************************** + * Enabling DebugP Log Support + ********************************************************************/ +var Log = xdc.useModule('xdc.runtime.Log'); +var Main = xdc.useModule('xdc.runtime.Main'); +var Diags = xdc.useModule('xdc.runtime.Diags'); +var LoggerBuf = xdc.useModule('xdc.runtime.LoggerBuf'); + +/* Configure the Logger Buffer: */ +var loggerBufParams = new LoggerBuf.Params(); +loggerBufParams.bufType = LoggerBuf.BufType_CIRCULAR; +loggerBufParams.exitFlush = false; +loggerBufParams.instance.name = "_logInfo"; +loggerBufParams.numEntries = 200; + +/* Create the Logger and attach this to the application */ +MyAppLogger = LoggerBuf.create(loggerBufParams); +Main.common$.logger = MyAppLogger; +Main.common$.diags_USER1 = Diags.RUNTIME_ON; +Task.common$.diags_USER1 = Diags.RUNTIME_ON; + diff --git a/mss/pcount3D_mss.h b/mss/pcount3D_mss.h new file mode 100644 index 0000000..5def42c --- /dev/null +++ b/mss/pcount3D_mss.h @@ -0,0 +1,394 @@ +/** + * @file pcount3D_mss.h + * + * @brief + * This is the main header file for the 3D people counting demo + * + * \par + * NOTE: + * (C) Copyright 2016 Texas Instruments, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef MMW_MSS_H +#define MMW_MSS_H + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +//#include + +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + +/*! @brief For advanced frame config, below define means the configuration given is + * global at frame level and therefore it is broadcast to all sub-frames. + */ +#define PCOUNT3DDEMO_SUBFRAME_NUM_FRAME_LEVEL_CONFIG (-1) + +/** + * @defgroup configStoreOffsets Offsets for storing CLI configuration + * @brief Offsets of config fields within the parent structures, note these offsets will be + * unique and hence can be used to differentiate the commands for processing purposes. + * @{ + */ +#define PCOUNT3DDEMO_ADCBUFCFG_OFFSET (offsetof(Pcount3DDemo_SubFrameCfg, adcBufCfg)) + +#define PCOUNT3DDEMO_SUBFRAME_DSPDYNCFG_OFFSET (offsetof(Pcount3DDemo_SubFrameCfg, objDetDynCfg) + \ + offsetof(Pcount3DDemo_DPC_ObjDet_DynCfg, dspDynCfg)) + +#define PCOUNT3DDEMO_SUBFRAME_R4FDYNCFG_OFFSET (offsetof(Pcount3DDemo_SubFrameCfg, objDetDynCfg) + \ + offsetof(Pcount3DDemo_DPC_ObjDet_DynCfg, r4fDynCfg)) + +#define PCOUNT3DDEMO_CALIBDCRANGESIG_OFFSET (PCOUNT3DDEMO_SUBFRAME_R4FDYNCFG_OFFSET + \ + offsetof(DPC_ObjectDetectionRangeHWA_DynCfg, calibDcRangeSigCfg)) + +#define PCOUNT3DDEMO_CAPONCHAINCFG_OFFSET (PCOUNT3DDEMO_SUBFRAME_DSPDYNCFG_OFFSET + \ + offsetof(DPC_ObjectDetection_DynCfg, caponChainCfg)) + +#define PCOUNT3DDEMO_DYNRACFARCFG_OFFSET (PCOUNT3DDEMO_CAPONCHAINCFG_OFFSET + \ + offsetof(caponChainCfg, dynamicCfarConfig)) + +#define PCOUNT3DDEMO_STATICRACFARCFG_OFFSET (PCOUNT3DDEMO_CAPONCHAINCFG_OFFSET + \ + offsetof(caponChainCfg, staticCfarConfig)) + +#define PCOUNT3DDEMO_DOACAPONCFG_OFFSET (PCOUNT3DDEMO_CAPONCHAINCFG_OFFSET + \ + offsetof(caponChainCfg, doaConfig)) + +#define PCOUNT3DDEMO_DOACAPONRACFG_OFFSET (PCOUNT3DDEMO_DOACAPONCFG_OFFSET + \ + offsetof(doaConfig, rangeAngleCfg)) + +#define PCOUNT3DDEMO_DOA2DESTCFG_OFFSET (PCOUNT3DDEMO_DOACAPONCFG_OFFSET + \ + offsetof(doaConfig, angle2DEst)) + +#define PCOUNT3DDEMO_DOAFOVCFG_OFFSET (PCOUNT3DDEMO_DOACAPONCFG_OFFSET + \ + offsetof(doaConfig, fovCfg)) + +#define PCOUNT3DDEMO_STATICANGESTCFG_OFFSET (PCOUNT3DDEMO_DOACAPONCFG_OFFSET + \ + offsetof(doaConfig, staticEstCfg)) + +#define PCOUNT3DDEMO_DOPCFARCFG_OFFSET (PCOUNT3DDEMO_DOACAPONCFG_OFFSET + \ + offsetof(doaConfig, dopCfarCfg)) + + + +/** @}*/ /* configStoreOffsets */ + +/** + * @brief + * 3D people counting Demo Sensor State + * + * @details + * The enumeration is used to define the sensor states used in 3D people counting Demo + */ +typedef enum Pcount3DDemo_SensorState_e +{ + /*! @brief Inital state after sensor is initialized. + */ + Pcount3DDemo_SensorState_INIT = 0, + + /*! @brief Indicates sensor is started */ + Pcount3DDemo_SensorState_STARTED, + + /*! @brief State after sensor has completely stopped */ + Pcount3DDemo_SensorState_STOPPED +}Pcount3DDemo_SensorState; + +/** + * @brief + * 3D people counting Demo statistics + * + * @details + * The structure is used to hold the statistics information for the + * 3D people counting Demo + */ +typedef struct Pcount3DDemo_MSS_Stats_t +{ + /*! @brief Counter which tracks the number of frame trigger events from BSS */ + uint64_t frameTriggerReady; + + /*! @brief Counter which tracks the number of failed calibration reports + * The event is triggered by an asynchronous event from the BSS */ + uint32_t failedTimingReports; + + /*! @brief Counter which tracks the number of calibration reports received + * The event is triggered by an asynchronous event from the BSS */ + uint32_t calibrationReports; + + /*! @brief Counter which tracks the number of sensor stop events received + * The event is triggered by an asynchronous event from the BSS */ + uint32_t sensorStopped; + + /*! @brief Counter which tracks the number of dpm stop events received + * The event is triggered by DPM_Report_DPC_STOPPED from DPM */ + uint32_t dpmStopEvents; + + /*! @brief Counter which tracks the number of dpm start events received + * The event is triggered by DPM_Report_DPC_STARTED from DPM */ + uint32_t dpmStartEvents; + +}Pcount3DDemo_MSS_Stats; + +/** + * @brief + * 3D people counting Demo Data Path Information. + * + * @details + * The structure is used to hold all the relevant information for + * the data path. + */ +typedef struct Pcount3DDemo_SubFrameCfg_t +{ + /*! @brief ADC buffer configuration storage */ + MmwDemo_ADCBufCfg adcBufCfg; + + /*! @brief Flag indicating if @ref adcBufCfg is pending processing. */ + uint8_t isAdcBufCfgPending : 1; + + /*! @brief Dynamic configuration storage for object detection DPC */ + Pcount3DDemo_DPC_ObjDet_DynCfg objDetDynCfg; + + /*! @brief ADCBUF will generate chirp interrupt event every this many chirps - chirpthreshold */ + uint8_t numChirpsPerChirpEvent; + + /*! @brief Number of bytes per RX channel, it is aligned to 16 bytes as required by ADCBuf driver */ + uint32_t adcBufChanDataSize; + + /*! @brief Number of ADC samples */ + uint16_t numAdcSamples; + + /*! @brief Number of chirps per sub-frame */ + uint16_t numChirpsPerSubFrame; + + /*! @brief Number of virtual antennas */ + uint8_t numVirtualAntennas; +} Pcount3DDemo_SubFrameCfg; + +/*! + * @brief + * Structure holds message stats information from data path. + * + * @details + * The structure holds stats information. This is a payload of the TLV message item + * that holds stats information. + */ +typedef struct Pcount3DDemo_SubFrameStats_t +{ + /*! @brief Frame processing stats */ + Pcount3DDemo_output_message_stats outputStats; + + /*! @brief Dynamic CLI configuration time in usec */ + uint32_t pendingConfigProcTime; + + /*! @brief SubFrame Preparation time on MSS in usec */ + uint32_t subFramePreparationTime; +} Pcount3DDemo_SubFrameStats; + +/** + * @brief Task handles storage structure + */ +typedef struct Pcount3DDemo_TaskHandles_t +{ + /*! @brief MMWAVE Control Task Handle */ + Task_Handle mmwaveCtrl; + + /*! @brief ObjectDetection DPC related dpmTask */ + Task_Handle objDetDpmTask; + + /*! @brief Demo init task */ + Task_Handle initTask; +} Pcount3DDemo_taskHandles; + +typedef struct Pcount3DDemo_DataPathObj_t +{ + /*! @brief Handle to hardware accelerator driver. */ + HWA_Handle hwaHandle; + + /*! @brief Handle of the EDMA driver. */ + EDMA_Handle edmaHandle; + + /*! @brief Radar cube memory information from range DPC */ + DPC_ObjectDetectionRangeHWA_preStartCfg_radarCubeMem radarCubeMem; + + /*! @brief Memory usage after the preStartCfg range DPC is applied */ + DPC_ObjectDetectionRangeHWA_preStartCfg_memUsage memUsage; + + /*! @brief EDMA error Information when there are errors like missing events */ + EDMA_errorInfo_t EDMA_errorInfo; + + /*! @brief EDMA transfer controller error information. */ + EDMA_transferControllerErrorInfo_t EDMA_transferControllerErrorInfo; + +} Pcount3DDemo_DataPathObj; + +/** + * @brief + * 3D people counting Demo MCB + * + * @details + * The structure is used to hold all the relevant information for the + * 3D people counting Demo + */ +typedef struct Pcount3DDemo_MSS_MCB_t +{ + /*! @brief Configuration which is used to execute the demo */ + Pcount3DDemo_Cfg cfg; + + /*! * @brief Handle to the SOC Module */ + SOC_Handle socHandle; + + /*! @brief UART Logging Handle */ + UART_Handle loggingUartHandle; + + /*! @brief UART Command Rx/Tx Handle */ + UART_Handle commandUartHandle; + + /*! @brief This is the mmWave control handle which is used + * to configure the BSS. */ + MMWave_Handle ctrlHandle; + + /*! @brief ADCBuf driver handle */ + ADCBuf_Handle adcBufHandle; + + /*! @brief DSP chain DPM Handle */ + DPM_Handle objDetDpmHandle; + + /*! @brief Object Detection DPC common configuration */ + Pcount3DDemo_DPC_ObjDet_CommonCfg objDetCommonCfg; + + /*! @brief Data path object */ + Pcount3DDemo_DataPathObj dataPathObj; + + /*! @brief Tracker DPU Static Configuration */ + DPC_ObjectDetection_TrackerConfig trackerCfg; + + /*! @brief Object Detection DPC subFrame configuration */ + Pcount3DDemo_SubFrameCfg subFrameCfg[RL_MAX_SUBFRAMES]; + + /*! @brief sub-frame stats */ + Pcount3DDemo_SubFrameStats subFrameStats[RL_MAX_SUBFRAMES]; + + /*! @brief Demo Stats */ + Pcount3DDemo_MSS_Stats stats; + + Pcount3DDemo_output_message_UARTpointCloud pointCloudToUart; + DPIF_DetMatrix heatMapOutFromDSP; + DPIF_PointCloudSpherical *pointCloudFromDSP; + DPIF_PointCloudSideInfo *pointCloudSideInfoFromDSP; + DPC_ObjectDetection_Stats *frameStatsFromDSP; + uint32_t currSubFrameIdx; + + trackerProc_TargetDescrHandle trackerOutput; + uint8_t numTargets; + uint16_t numIndices; + bool presenceDetEnabled; + uint32_t presenceInd; + uint16_t numDetectedPoints; + uint32_t trackerProcessingTimeInUsec; + uint32_t uartProcessingTimeInUsec; + + + /*! @brief Task handle storage */ + Pcount3DDemo_taskHandles taskHandles; + + /*! @brief RF frequency scale factor, = 2.7 for 60GHz device, = 3.6 for 76GHz device */ + double rfFreqScaleFactor; + + /*! @brief Semaphore handle to signal DPM started from DPM report function */ + Semaphore_Handle DPMstartSemHandle; + + /*! @brief Semaphore handle to signal DPM stopped from DPM report function. */ + Semaphore_Handle DPMstopSemHandle; + + /*! @brief Semaphore handle to signal DPM ioctl from DPM report function. */ + Semaphore_Handle DPMioctlSemHandle; + + /*! @brief Semaphore handle to run UART DMA task. */ + Semaphore_Handle uartTxSemHandle; + + /*! @brief Semaphore handle to trigger tracker DPU. */ + Semaphore_Handle trackerDPUSemHandle; + + /*! @brief Sensor state */ + Pcount3DDemo_SensorState sensorState; + + /*! @brief Tracks the number of sensor start */ + uint32_t sensorStartCount; + + /*! @brief Tracks the number of sensor sop */ + uint32_t sensorStopCount; + +} Pcount3DDemo_MSS_MCB; + +/************************************************************************** + *************************** Extern Definitions *************************** + **************************************************************************/ + +/* Functions to handle the actions need to move the sensor state */ +extern int32_t Pcount3DDemo_openSensor(bool isFirstTimeOpen); +extern int32_t Pcount3DDemo_configSensor(void); +extern int32_t Pcount3DDemo_startSensor(void); +extern void Pcount3DDemo_stopSensor(void); + +/* functions to manage the dynamic configuration */ +extern uint8_t Pcount3DDemo_isAllCfgInPendingState(void); +extern uint8_t Pcount3DDemo_isAllCfgInNonPendingState(void); +extern void Pcount3DDemo_resetStaticCfgPendingState(void); +extern void Pcount3DDemo_CfgUpdate(void *srcPtr, uint32_t offset, uint32_t size, int8_t subFrameNum); + +extern void Pcount3DDemo_CLIInit (uint8_t taskPriority); + +/* Debug Functions */ +extern void _Pcount3DDemo_debugAssert(int32_t expression, const char *file, int32_t line); +#define Pcount3DDemo_debugAssert(expression) { \ + _Pcount3DDemo_debugAssert(expression, \ + __FILE__, __LINE__); \ + DebugP_assert(expression); \ + } + +#ifdef __cplusplus +} +#endif + +#endif /* MMW_MSS_H */ + diff --git a/mss/pcount3D_mss_linker.cmd b/mss/pcount3D_mss_linker.cmd new file mode 100644 index 0000000..3c64c1f --- /dev/null +++ b/mss/pcount3D_mss_linker.cmd @@ -0,0 +1,16 @@ +/*----------------------------------------------------------------------------*/ +/* Linker Settings */ +--retain="*(.intvecs)" + +/*----------------------------------------------------------------------------*/ +/* Section Configuration */ +SECTIONS +{ + systemHeap : {} > DATA_RAM + .hwaBufs: load = HWA_RAM, type = NOINIT + .dpcLocalRam: {} > DATA_RAM + .l3ram: {} >> L3_RAM + .demoSharedMem: { } >> HS_RAM +} +/*----------------------------------------------------------------------------*/ + diff --git a/mss/r4f_linker.cmd b/mss/r4f_linker.cmd new file mode 100644 index 0000000..9280ac1 --- /dev/null +++ b/mss/r4f_linker.cmd @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* r4f_linker.cmd */ +/* */ +/* (c) Texas Instruments 2016, All rights reserved. */ +/* */ + +/* USER CODE BEGIN (0) */ +/* USER CODE END */ + + +/*----------------------------------------------------------------------------*/ +/* Linker Settings */ +--retain="*(.intvecs)" + +/*----------------------------------------------------------------------------*/ +/* Memory Map */ +#define MMWAVE_L3RAM_SIZE (MMWAVE_L3RAM_NUM_BANK*MMWAVE_SHMEM_BANK_SIZE - MMWAVE_MSSUSED_L3RAM_SIZE) +MEMORY{ +PAGE 0: + VECTORS (X) : origin=0x00000000 length=0x00000100 + PROG_RAM (RX) : origin=0x00000100 length=0x0007FF00+(MMWAVE_SHMEM_TCMA_NUM_BANK*MMWAVE_SHMEM_BANK_SIZE) + DATA_RAM (RW) : origin=0x08000000 length=0x00030000+(MMWAVE_SHMEM_TCMB_NUM_BANK*MMWAVE_SHMEM_BANK_SIZE) + L3_RAM (RW) : origin=0x51000000 length=0x00000000+(MMWAVE_MSSUSED_L3RAM_SIZE) + HWA_RAM (RW) : origin=0x52030000 length=0x00010000 + HS_RAM (RW) : origin=0x52080000 length=0x8000 +PAGE 1: + L3_RAM (RW) : origin=0x51000000 length=0x00000000+(MMWAVE_MSSUSED_L3RAM_SIZE) +} + +/*----------------------------------------------------------------------------*/ +/* Section Configuration */ +SECTIONS{ + .intvecs : {} > VECTORS + .text : {} > PROG_RAM + .const : {} > PROG_RAM + .cinit : {} > PROG_RAM + .pinit : {} > PROG_RAM + .bss : {} > DATA_RAM + .data : {} > DATA_RAM + .stack : {} > DATA_RAM +} +/*----------------------------------------------------------------------------*/ + diff --git a/mss/src/.exclude b/mss/src/.exclude new file mode 100644 index 0000000..8c86331 --- /dev/null +++ b/mss/src/.exclude @@ -0,0 +1 @@ +This file exists to prevent Eclipse/CDT from adding the C sources contained in this directory (or below) to any enclosing project. diff --git a/mss/src/makefile.libs b/mss/src/makefile.libs new file mode 100644 index 0000000..e03f88f --- /dev/null +++ b/mss/src/makefile.libs @@ -0,0 +1,62 @@ +# +# This file was generated based on the configuration script: +# C:\Users\Daunair\workspace_v10\3D_people_count_68xx_mss\mss\pcount3D_mss.cfg +# +# This makefile may be included in other makefiles that need to build +# the libraries containing the compiled source files generated as +# part of the configuration step. + +# +# ======== GEN_SRC_DIR ========= +# The path to the sources generated during configuration +# +# This path must be either absolute or relative to the build directory. +# +# The absolute path to the generated source directory (at the time the +# sources were generated) is: +# C:\Users\Daunair\workspace_v10\3D_people_count_68xx_mss\mss\src +# +GEN_SRC_DIR ?= ../mss/src + +ifeq (,$(wildcard $(GEN_SRC_DIR))) +$(error "ERROR: GEN_SRC_DIR must be set to the directory containing the generated sources") +endif + +# +# ======== .force ======== +# The .force goal is used to force the build of any goal that names it as +# a prerequisite +# +.PHONY: .force + +# +# ======== library macros ======== +# +sysbios_SRC = $(GEN_SRC_DIR)/sysbios +sysbios_LIB = $(GEN_SRC_DIR)/sysbios/sysbios.aer4ft + +# +# ======== dependencies ======== +# +all: $(sysbios_LIB) +clean: .sysbios_clean + + +# ======== convenient build goals ======== +.PHONY: sysbios +sysbios: $(GEN_SRC_DIR)/sysbios/sysbios.aer4ft + +# CDT managed make executables depend on $(OBJS) +OBJS += $(sysbios_LIB) + +# +# ======== rules ======== +# +$(sysbios_LIB): .force + @echo making $@ ... + @$(MAKE) -C $(sysbios_SRC) + +.sysbios_clean: + @echo cleaning $(sysbios_SRC) ... + -@$(MAKE) --no-print-directory -C $(sysbios_SRC) clean + diff --git a/mss/src/sysbios/makefile b/mss/src/sysbios/makefile new file mode 100644 index 0000000..0acff14 --- /dev/null +++ b/mss/src/sysbios/makefile @@ -0,0 +1,82 @@ +# This is a generated file. +# +# Do not edit this file. Any modifications to this file +# will be overwritten whenever makefiles are re-generated. +# +# template: ti.sysbios/makefile.xdt +# target: ti.targets.arm.elf.R4Ft + +vpath % C:/ti/bios_6_73_01_01/packages/ti/sysbios/ +vpath %.c C:/ti/xdctools_3_50_08_24_core/packages/ + +XOPTS = -I"C:/ti/xdctools_3_50_08_24_core/packages/" -Dxdc_target_types__=C:/ti/bios_6_73_01_01/packages/ti/targets/arm/elf/std.h -Dxdc_target_name__=R4Ft + +BIOS_DEFS = -Dti_sysbios_BIOS_swiEnabled__D=FALSE -Dti_sysbios_BIOS_taskEnabled__D=TRUE -Dti_sysbios_BIOS_clockEnabled__D=TRUE -Dti_sysbios_BIOS_runtimeCreatesEnabled__D=TRUE -Dti_sysbios_knl_Task_moduleStateCheckFlag__D=FALSE -Dti_sysbios_knl_Task_objectCheckFlag__D=FALSE -Dti_sysbios_hal_Hwi_DISABLE_ALL_HOOKS -Dti_sysbios_knl_Swi_DISABLE_ALL_HOOKS -Dti_sysbios_BIOS_smpEnabled__D=FALSE -Dti_sysbios_BIOS_mpeEnabled__D=FALSE -Dti_sysbios_Build_useHwiMacros -Dti_sysbios_knl_Swi_numPriorities__D=16 -Dti_sysbios_knl_Task_deleteTerminatedTasks__D=FALSE -Dti_sysbios_knl_Task_numPriorities__D=16 -Dti_sysbios_knl_Task_checkStackFlag__D=TRUE -Dti_sysbios_knl_Task_initStackFlag__D=TRUE -Dti_sysbios_knl_Clock_TICK_SOURCE=ti_sysbios_knl_Clock_TickSource_TIMER -Dti_sysbios_knl_Clock_TICK_MODE=ti_sysbios_knl_Clock_TickMode_PERIODIC -Dti_sysbios_hal_Core_delegate_getId=ti_sysbios_family_arm_v7r_tms570_Core_getId__E -Dti_sysbios_hal_Core_delegate_interruptCore=ti_sysbios_family_arm_v7r_tms570_Core_interruptCore__E -Dti_sysbios_hal_Core_delegate_lock=ti_sysbios_family_arm_v7r_tms570_Core_lock__E -Dti_sysbios_hal_Core_delegate_unlock=ti_sysbios_family_arm_v7r_tms570_Core_unlock__E -Dti_sysbios_hal_Core_numCores__D=1 -Dti_sysbios_family_arm_v7r_tms570_Core_numCores__D=1 -Dti_sysbios_utils_Load_taskEnabled__D=TRUE -Dti_sysbios_utils_Load_swiEnabled__D=FALSE -Dti_sysbios_utils_Load_hwiEnabled__D=FALSE -Dti_sysbios_family_arm_v7r_vim_Hwi_dispatcherSwiSupport__D=FALSE -Dti_sysbios_family_arm_v7r_vim_Hwi_dispatcherTaskSupport__D=TRUE -Dti_sysbios_family_arm_v7r_vim_Hwi_dispatcherAutoNestingSupport__D=TRUE -Dti_sysbios_family_arm_v7r_vim_Hwi_dispatcherIrpTrackingSupport__D=TRUE -Dti_sysbios_knl_Semaphore_supportsEvents__D=FALSE -Dti_sysbios_knl_Semaphore_supportsPriority__D=TRUE + +CCOPTS = --code_state=16 --float_support=vfpv3d16 --endian=little -mv7R4 --abi=eabi -q -ms --opt_for_speed=2 --program_level_compile -o3 -g --enum_type=int -Dti_sysbios_family_arm_exc_Exception_enableDecode__D=TRUE -Dti_sysbios_knl_Clock_stopCheckNext__D=FALSE -Dti_sysbios_family_arm_v7r_vim_Hwi_lockstepDevice__D=TRUE -Dti_sysbios_family_arm_v7r_vim_Hwi_errataInitEsm__D=TRUE -Dti_sysbios_family_arm_v7r_vim_Hwi_resetVIM__D=TRUE -Dti_sysbios_hal_Core_numCores__D=1 -Dti_sysbios_knl_Task_ENABLE_SWITCH_HOOKS -Dti_sysbios_knl_Task_minimizeLatency__D=FALSE + +BIOS_INC = -I"C:/ti/bios_6_73_01_01/packages/" +ANNEX_INCS = +INCS = $(BIOS_INC) $(ANNEX_INCS) -I"C:/ti/bios_6_73_01_01/packages/" -I"C:/ti/xdctools_3_50_08_24_core/packages/" -I"../" -I"/packages//" -IC:/ti/ti-cgt-arm_16.9.6.LTS/include + +CC = C:/ti/ti-cgt-arm_16.9.6.LTS/bin/armcl -c $(CCOPTS) +ASM = C:/ti/ti-cgt-arm_16.9.6.LTS/bin/armcl -c $(CCOPTS) +AR = C:/ti/ti-cgt-arm_16.9.6.LTS/bin/armar rq + +DEL = C:/ti/xdctools_3_50_08_24_core/packages/../bin/rm -f +CP = C:/ti/xdctools_3_50_08_24_core/packages/../bin/cp -f + +define RM + $(if $(wildcard $1),$(DEL) $1,:) +endef + +define ASSEMBLE + @echo asmer4ft $< ... + @$(ASM) $(BIOS_DEFS) $(XOPTS) $(INCS) $< +endef + +all: sysbios.aer4ft + +arm_IntrinsicsSupport_asm.obj: family/arm/IntrinsicsSupport_asm.asm makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=arm_IntrinsicsSupport_asm.obj + +arm_TaskSupport_asm.obj: family/arm/TaskSupport_asm.asm makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=arm_TaskSupport_asm.obj + +vim_Hwi_asm.obj: family/arm/v7r/vim/Hwi_asm.sv7R makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=vim_Hwi_asm.obj + +vim_Hwi_asm_switch.obj: family/arm/v7r/vim/Hwi_asm_switch.sv7R makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=vim_Hwi_asm_switch.obj + +exc_Exception_asm.obj: family/arm/exc/Exception_asm.asm makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=exc_Exception_asm.obj + +tms570_Core_asm.obj: family/arm/v7r/tms570/Core_asm.sv7R makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=tms570_Core_asm.obj + +a15_TimestampProvider_asm.obj: family/arm/a15/TimestampProvider_asm.asm makefile + @-$(call RM, $@) + $(ASSEMBLE) --output_file=a15_TimestampProvider_asm.obj + + +BIOS.obj: BIOS.c family/arm/IntrinsicsSupport.c family/arm/TaskSupport.c knl/Clock.c knl/Idle.c knl/Intrinsics.c knl/Event.c knl/Queue.c knl/Semaphore.c knl/Task.c heaps/HeapBuf.c heaps/HeapMem.c family/arm/v7r/vim/Hwi.c family/arm/exc/Exception.c hal/Cache.c hal/CacheNull.c hal/Core.c hal/Hwi.c hal/Hwi_stack.c hal/Hwi_startup.c family/arm/v7a/Pmu.c utils/Load.c utils/Load_CPU.c gates/GateHwi.c gates/GateMutex.c family/arm/v7r/tms570/Core.c timers/rti/Timer.c family/arm/a15/TimestampProvider.c xdc/runtime/xdc_noinit.c xdc/runtime/Assert.c xdc/runtime/Core-mem.c xdc/runtime/Core-smem.c xdc/runtime/Core-label.c xdc/runtime/Core-params.c xdc/runtime/Diags.c xdc/runtime/Error.c xdc/runtime/Gate.c xdc/runtime/Log.c xdc/runtime/LoggerBuf.c xdc/runtime/Memory.c xdc/runtime/Registry.c xdc/runtime/Startup.c xdc/runtime/System.c xdc/runtime/SysStd.c xdc/runtime/Text.c xdc/runtime/Timestamp.c xdc/runtime/TimestampNull.c makefile + @-$(call RM, $@) + @echo cler4ft $< ... + @$(CC) $(BIOS_DEFS) $(XOPTS) $(INCS) $(subst makefile,,$^) + +sysbios.aer4ft: BIOS.obj arm_IntrinsicsSupport_asm.obj arm_TaskSupport_asm.obj vim_Hwi_asm.obj vim_Hwi_asm_switch.obj exc_Exception_asm.obj tms570_Core_asm.obj a15_TimestampProvider_asm.obj + @-$(call RM, $@) + @echo arer4ft $^ ... + @$(AR) $@ $^ + + +clean: + @$(DEL) ..\makefile.libs + @-$(call RM, *) diff --git a/mss/src/sysbios/sysbios.aer4ft b/mss/src/sysbios/sysbios.aer4ft new file mode 100644 index 0000000..e05b6fa Binary files /dev/null and b/mss/src/sysbios/sysbios.aer4ft differ diff --git a/mss/tracker_output.h b/mss/tracker_output.h new file mode 100644 index 0000000..34af10b --- /dev/null +++ b/mss/tracker_output.h @@ -0,0 +1,99 @@ +/** + * @file mmw_output.h + * + * @brief + * This is the interface/message header file for the Millimeter Wave Demo + * + * \par + * NOTE: + * (C) Copyright 2016 Texas Instruments, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef TRACKER_OUTPUT_H +#define TRACKER_OUTPUT_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** @brief Output packet length is a multiple of this value, must be power of 2*/ +#define TRACKERDEMO_OUTPUT_MSG_SEGMENT_LEN 32 +/*! + * @brief + * Message header for reporting detection information from data path. + * + * @details + * The structure defines the message header. + */ +typedef struct TrackerDemo_output_message_header_t +{ + /*! @brief Output buffer magic word (sync word). It is initialized to {0x0102,0x0304,0x0506,0x0708} */ + uint16_t magicWord[4]; + + /*! @brief SW Version: : MajorNum * 2^24 + MinorNum * 2^16 + BugfixNum * 2^8 + BuildNum */ + uint32_t version; + + /*! @brief HW platform type */ + uint32_t platform; + + /*! @brief Time in CPU cycles when the message was created, R4F CPU cycles */ + uint32_t timeStamp; + + /*! @brief Total packet length including header in Bytes */ + uint32_t totalPacketLen; + + /*! @brief Frame number */ + uint32_t frameNumber; + + /*! @brief For Advanced Frame config, this is the sub-frame number in the range + * 0 to (number of subframes - 1). For frame config (not advanced), this is always + * set to 0. */ + uint32_t subFrameNumber; + + /*! @brief Detection Layer Margins */ + uint32_t chirpProcessingMargin; + uint32_t frameProcessingMargin; + + /*! @brief Localization Layer Timing */ + uint32_t trackingProcessingTime; + uint32_t uartSendingTime; + + /*! @brief Number of TLVs in this message*/ + uint16_t numTLVs; + /*! @brief Header checksum */ + uint16_t checksum; + +} TrackerDemo_output_message_header; + +#ifdef __cplusplus +} +#endif + +#endif /* MMW_OUTPUT_H */ diff --git a/mss/tracker_utils.c b/mss/tracker_utils.c new file mode 100644 index 0000000..aa69872 --- /dev/null +++ b/mss/tracker_utils.c @@ -0,0 +1,580 @@ +/** + * @file task_mbox.c + * + * @brief + * MSS main implementation of the millimeter wave Demo + * + * Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/************************************************************************** + *************************** Include Files ******************************** + **************************************************************************/ + +/* Standard Include Files. */ +#include +#include +#include +#include +#include +#include + + +/* BIOS/XDC Include Files. */ +#include + +/* mmWave SDK Include Files: */ +#include +#include +#include +#include +#include +#include "float.h" + +/* Demo Include Files */ +#include + + +/************************************************************************** + *************************** Local Definitions **************************** + **************************************************************************/ + +/************************************************************************** + *************************** Global Definitions *************************** + **************************************************************************/ + +typedef enum { + TRACKING_DEFAULT_PARAM_SET = 0, + TRACKING_TRAFFIC_MONITORING_PARAM_SET, + TRACKING_PEOPLE_COUNTING_PARAM_SET, + TRACKING_OUTDOOR_PARAM_SET, + TRACKING_CEILING_MOUNT_PARAM_SET +} TRACKING_ADVANCED_PARAM_SET; + +typedef enum { + TRACKING_PARAM_SET_TM = 0, + TRACKING_PARAM_SET_PC, + TRACKING_PARAM_SET_OUTDOOR, + TRACKING_PARAM_SET_CEILING_MOUNT +} TRACKING_ADVANCED_PARAM_SET_TABLE; +#if 0 +/* Scenery parameters includes up to two boundary boxes and up to two static boxes */ +/* Each box is in format {x1,x2, y1,y2, z1,z2}. In 2D cases, the z parameters are ignored */ +GTRACK_sceneryParams appSceneryParamTable[4] = { + /* TM: 1 boundary box: {-1,12, 15,75, 0,0}, and 1 static box {0,14, 19,50, 0,0} */ + 1,{{-1.f,12.f, 15.f,75.f, 0.f,0.f},{0.f,0.f,0.f,0.f,0.f,0.f}},1,{{0.f,11.f, 19.f,50.f, 0.f,0.f},{0.f,0.f,0.f,0.f,0.f}}, + /* PEOPLE COUNTING: 1 boundary box: {-4,4, 0.5,7.5, 0,0}, and 1 static box {-3,3, 2,6, 0,0} */ + 1,{{-4.f,4.f, 0.5f,7.5f, 0.f,0.f},{0.f,0.f,0.f,0.f,0.f,0.f}}, 1,{{-3.f,3.f,2.f,6.f,0.f,0.f},{0.f,0.f,0.f,0.f,0.f,0.f}}, + /* OUTDOOR: 1 boundary box: {-39,19, 2,50, 0,0}, and 1 static box {-30,16, 4,44, 0,0} */ + 1,{{-29.f,39.f, 2.f,59.f, -1.f,3.f},{0.f,0.f,0.f,0.f,0.f,0.f}}, 1,{{-20.f,20.f, 12.f,40.f, 0.f, 2.f},{0.f,0.f,0.f,0.f,0.f,0.f}}, + /* CEILING MOUNT: 1 boundary box: {-4,4, 0.5,7.5, -1,3}, and 1 static box {-3,3, 2,6, -0.5,2.5} */ + 1,{{-4.f,4.f, 0.5f,7.5f, -1.f,3.0f},{0.f,0.f,0.f,0.f,0.f,0.f}}, 1,{{-3.f,3.f,2.f,6.f,-0.5,2.5f},{0.f,0.f,0.f,0.f,0.f,0.f}} +}; + +/* Gating Volume 2 "liters", Limits are set to 2m in depth and width, no limit in height and doppler */ +GTRACK_gatingParams appGatingParamTable[4] = { + /* TM: Gating volume = 16, Limits are set to 12m in depth, 8m in width, ignore the height, no limit in doppler */ + {4.f, {12.f, 6.f, 4.f, 12.f}}, + /* PEOPLE COUNTING: Gating gain = 3, Limits are 2m in depth, 2m in width, ignore the height, 12m/s in doppler */ + {3.f, {2.f, 2.f, 2.f, 12.f}}, + /* OUTDOOR: Gating gain = 4, Limits are set to 6m in depth, 6m in width, ignore the height, 10m/s limit in doppler */ + {4.f, {6.f, 6.f, 4.f, 10.f}}, + /* CEILING MOUNT: Gating volume = 2, Limits are 2m in depth, 2m in width, 2m the height, no limit in doppler */ + {2.f, {2.f, 2.f, 2.f, 10.f}} +}; +GTRACK_stateParams appStateParamTable[4] = { + {3U, 3U, 5U, 100U, 5U}, /* TM: det2act, det2free, act2free, stat2free, exit2free */ + {10U, 5U, 50U, 100U, 5U}, /* PC: det2act, det2free, act2free, stat2free, exit2free */ + {4U, 10U, 60U, 600U, 20U}, /* OUTDOOR: det2act, det2free, act2free, stat2free, exit2free */ + {10U, 5U, 10U, 100U, 5U} /* CEILING MOUNT: det2act, det2free, act2free, stat2free, exit2free */ +}; +GTRACK_allocationParams appAllocationParamTable[4] = { + {100.f, 100.f, 1.f, 3U, 4.f, 2.f}, /* TM: 100 (100) SNRs, 1m/s minimal velocity, 3 points with 4m in distance, 2m/c in velocity separation */ + {60.f, 200.f, 0.1f, 5U, 1.5f, 2.f}, /* PC: 150 (250 obscured), 0.1 m/s minimal velocity, 5 points, with 1m in distance, 2m/c in velocity in separation */ + {40.f, 200.f, 0.5f, 3U, 2.f, 2.f}, /* OUTDOOR: 50 (200 obscured), 0.5 m/s minimal velocity, 5 points, with 1m in distance, 2m/c in velocity in separation */ + {60.f, 200.f, 0.1f, 5U, 1.5f, 2.f} /* CEILING MOUNT: 150 (200 obscured), 0.5 m/s minimal velocity, 5 points, with 1m in distance, 2m/c in velocity in separation */ +}; +/* This parameter is ignored in 2D/3D tracker versions */ +GTRACK_varParams appVariationParamTable[4] = { + {0.f, 0.f, 0.f}, + {0.f, 0.f, 0.f}, + {0.f, 0.f, 0.f}, + {0.f, 0.f, 0.f} +}; + +float maxAccelerationParams[3] = {1, 1, 1}; +#endif +/** + * @brief + * Global Variable for tracking information required by the mmw Demo + */ +extern Pcount3DDemo_MSS_MCB gMmwMssMCB; + +unsigned int gGtrackMemoryUsed = 0; + +/* @TODO: These functions need to be abstracted to the DPC */ +void *gtrack_alloc(unsigned int numElements, unsigned int sizeInBytes) +{ + gGtrackMemoryUsed += numElements*sizeInBytes; + return MemoryP_ctrlAlloc(numElements*sizeInBytes, 0); +} +void gtrack_free(void *pFree, unsigned int sizeInBytes) +{ + gGtrackMemoryUsed -= sizeInBytes; + MemoryP_ctrlFree(pFree,sizeInBytes); +} +void gtrack_log(GTRACK_VERBOSE_TYPE level, const char *format, ...) +{ +#if 0 + va_list args; + va_start(args, format); + vprintf(format, args); + va_end(args); +#endif +} + +/** + * @b Description + * @n + * This is the CLI Handler for tracking configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLITrackingCfg (int32_t argc, char* argv[]) +{ + GTRACK_moduleConfig config; + + + TRACKING_ADVANCED_PARAM_SET trackingParamSet; + + //Memory_Stats startMemoryStats; + //Memory_Stats endMemoryStats; + + if (argc >= 1) { + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.trackerEnabled = (uint16_t) atoi (argv[1]); + } + + if(gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.trackerEnabled != 1) { + return 0; + } + + /* Sanity Check: Minimum argument check */ + if (argc != 8) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } +#if 0 + System_printf("Debug: Heap before creating a tracker\n"); + MmwDemo_printHeapStats(); +#endif + /* Initialize CLI configuration: */ + memset ((void *)&config, 0, sizeof(GTRACK_moduleConfig)); + + trackingParamSet = (TRACKING_ADVANCED_PARAM_SET) atoi (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.trackingParamSet = trackingParamSet; + + switch(trackingParamSet) + { + case TRACKING_DEFAULT_PARAM_SET: + // Do not configure advanced parameters, use library default parameters + config.advParams = 0; + break; + + case TRACKING_TRAFFIC_MONITORING_PARAM_SET: + /* Initialize CLI configuration: */ + config.initialRadialVelocity = -8.0f; // for TM, detected targets are approaching + config.maxAcceleration[0] = 2.0f; // for TM, maximum acceleration in lateral direction is set to 2m/s2 + config.maxAcceleration[1] = 20.0f; // for TM, maximum acceleration is longitudinal direction set to 20m/s2 + config.maxAcceleration[2] = 0.f; // ignored + break; + + case TRACKING_PEOPLE_COUNTING_PARAM_SET: + /* Initialize CLI configuration: */ + config.initialRadialVelocity = 0; //For PC, detected target velocity is unknown + config.maxAcceleration[0] = 0.1f; + config.maxAcceleration[1] = 0.1f; + config.maxAcceleration[2] = 0.1f; + break; + + case TRACKING_OUTDOOR_PARAM_SET: + /* Initialize CLI configuration: */ + config.initialRadialVelocity = 0; // for OUTDOOR, detected targets velocity is unknown + config.maxAcceleration[0] = 1.0f; + config.maxAcceleration[1] = 1.0f; + config.maxAcceleration[2] = 1.0f; + break; + + case TRACKING_CEILING_MOUNT_PARAM_SET: + /* Initialize CLI configuration: */ + config.initialRadialVelocity = 0; + config.maxAcceleration[0] = 0.1f; + config.maxAcceleration[1] = 0.1f; + config.maxAcceleration[2] = 0.1f; + break; + + + default: + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } +#ifdef GTRACK_3D + config.stateVectorType = GTRACK_STATE_VECTORS_3DA; // Track three dimensions with acceleration +#else + config.stateVectorType = GTRACK_STATE_VECTORS_2DA; // Track two dimensions with acceleration +#endif + config.verbose = GTRACK_VERBOSE_NONE; + config.maxNumPoints = (uint16_t) atoi(argv[3]); + config.maxNumTracks = (uint16_t) atoi(argv[4]); + config.maxRadialVelocity = (float) atoi(argv[5]) *0.1f; +#ifndef GTRACK_3D + config.radialVelocityResolution = (float) atoi(argv[6]) *0.001f; +#endif + config.deltaT = (float) atoi(argv[7]) *0.001f; + + /* Save Configuration to use later */ + memcpy((void *)&gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gtrackModuleConfig, (void *)&config, sizeof(GTRACK_moduleConfig)); + + return 0; +} + +int32_t MmwDemo_CLIStaticBoundaryBoxCfg (int32_t argc, char* argv[]) +{ + /* Sanity Check: Minimum argument check */ +#ifdef GTRACK_3D + if (argc != 7) +#else + if (argc != 5) +#endif + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + /* Initialize the ADC Output configuration: */ + //memset ((void *)&sceneryParams, 0, sizeof(GTRACK_sceneryParams)); + + /* Populate configuration: */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.numStaticBoxes = 1; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.staticBox[0].x1 = (float) atof (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.staticBox[0].x2 = (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.staticBox[0].y1 = (float) atof (argv[3]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.staticBox[0].y2 = (float) atof (argv[4]); + +#ifdef GTRACK_3D + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.staticBox[0].z1 = (float) atof (argv[5]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.staticBox[0].z2 = (float) atof (argv[6]); +#endif + + return 0; + +} + +int32_t MmwDemo_CLIBoundaryBoxCfg (int32_t argc, char* argv[]) +{ + /* Sanity Check: Minimum argument check */ +#ifdef GTRACK_3D + if (argc != 7 && argc != 13) +#else + if (argc != 5) +#endif + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + /* Initialize the ADC Output configuration: */ + //memset ((void *)&sceneryParams, 0, sizeof(GTRACK_sceneryParams)); + + /* Populate configuration: */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.numBoundaryBoxes = 1; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[0].x1 = (float) atof (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[0].x2 = (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[0].y1 = (float) atof (argv[3]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[0].y2 = (float) atof (argv[4]); +#ifdef GTRACK_3D + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[0].z1 = (float) atof (argv[5]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[0].z2 = (float) atof (argv[6]); +#endif + if (argc == 13) { //second boundary box + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.numBoundaryBoxes = 2; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[1].x1 = (float) atof (argv[7]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[1].x2 = (float) atof (argv[8]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[1].y1 = (float) atof (argv[9]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[1].y2 = (float) atof (argv[10]); + #ifdef GTRACK_3D + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[1].z1 = (float) atof (argv[11]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.boundaryBox[1].z2 = (float) atof (argv[12]); + #endif + } + + return 0; + +} + +int32_t MmwDemo_CLISensorPositionCfg(int32_t argc, char* argv[]) +{ + if (argc != 4) + { + CLI_write("Error: Invalid usage of the CLI Command\n"); + return -1; + } + + /* Assume sensor position as the origin in the xy plane so x=0, y=0*/ + /*populate sensor position configuration*/ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.sensorPosition.x = 0; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.sensorPosition.y = 0; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.sensorPosition.z = (float) atof (argv[1]); + /*populate sensor orientation configuration*/ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.sensorOrientation.azimTilt = (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.sensorOrientation.elevTilt = (float) atof (argv[3]); + /*demo parameters*/ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sensorAzimuthTilt = atoi(argv[2])*3.14f/180; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sensorElevationTilt = atoi(argv[3])*3.14f/180; + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sensorHeight = (float)atof(argv[1]); + + return 0; +} +/** + * @b Description + * @n + * This is the CLI Handler for GatingParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIGatingParamCfg (int32_t argc, char* argv[]) +{ + /* Sanity Check: Minimum argument check */ +#ifdef GTRACK_3D + if (argc != 6) +#else + if (argc != 5) +#endif + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + /* Initialize the ADC Output configuration: */ + //memset ((void *)&gatingParams, 0, sizeof(GTRACK_gatingParams)); + + /* Populate configuration: */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gatingParams.gain = (float) atof (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gatingParams.limits.width = (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gatingParams.limits.depth = (float) atof (argv[3]); +#ifdef GTRACK_3D + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gatingParams.limits.height = (float) atof (argv[4]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gatingParams.limits.vel = (float) atof (argv[5]); +#else + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.gatingParams.limits.vel = (float) atof (argv[4]); +#endif + + return 0; + +} + + +/** + * @b Description + * @n + * This is the CLI Handler for StateParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIStateParamCfg (int32_t argc, char* argv[]) +{ + /* Sanity Check: Minimum argument check */ + if (argc != 7) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + /* Initialize the ADC Output configuration: */ + //memset ((void *)&stateParams, 0, sizeof(GTRACK_stateParams)); + + /* Populate configuration: */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.stateParams.det2actThre = (uint16_t) atoi (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.stateParams.det2freeThre= (uint16_t) atoi (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.stateParams.active2freeThre= (uint16_t) atoi (argv[3]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.stateParams.static2freeThre= (uint16_t) atoi (argv[4]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.stateParams.exit2freeThre= (uint16_t) atoi (argv[5]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.stateParams.sleep2freeThre = (uint16_t) atoi (argv[6]); + + return 0; + +} + +/** + * @b Description + * @n + * This is the CLI Handler for AllocationParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIAllocationParamCfg (int32_t argc, char* argv[]) +{ + /* Sanity Check: Minimum argument check */ +#ifdef GTRACK_3D + if (argc != 7) +#else + if (argc != 6) +#endif + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + /* Initialize the ADC Output configuration: */ + //memset ((void *)&allocationParams, 0, sizeof(GTRACK_allocationParams)); + + /* Populate configuration: */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.snrThre = (float) atof (argv[1]); +#ifdef GTRACK_3D + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.snrThreObscured = (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.velocityThre = (float) atof (argv[3]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.pointsThre = (uint16_t) atoi (argv[4]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.maxDistanceThre = (float) atof (argv[5]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.maxVelThre = (float) atof (argv[6]); +#else + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.velocityThre = (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.pointsThre = (uint16_t) atoi (argv[3]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.maxDistanceThre = (float) atof (argv[4]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.allocationParams.maxVelThre = (float) atof (argv[5]); +#endif + + return 0; + +} + +int32_t MmwDemoCLIMaxAccelerationParamCfg(int32_t argc, char* argv[]) +{ +#ifdef GTRACK_3D + if (argc != 4) +#else + if (argc != 3) +#endif + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.accelerationParams[0] = (float) atof (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.accelerationParams[1] = (float) atof (argv[2]); +#ifdef GTRACK_3D + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.accelerationParams[2] = (float) atof (argv[3]); +#endif + + return 0; + +} + +/** + * @b Description + * @n + * This is the CLI Handler for PresenceParams configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIPresenceParamCfg (int32_t argc, char* argv[]) +{ + /* Sanity Check: Minimum argument check */ + if (argc != 7) + { + CLI_write ("Error: Invalid usage of the CLI command\n"); + return -1; + } + + /* Hardcode presence detection thresholds */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.pointsThre= 3;//(uint16_t) atoi (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.velocityThre= 0.5;//(uint16_t) atoi (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.on2offThre= 10;//(uint16_t) atoi (argv[3]); + /* Only one presence detection boundary box supported for now */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.numOccupancyBoxes= 1; + + /* Set presence detection enabled flag */ + if( ( gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.sceneryParams.numBoundaryBoxes > 0) + && ( gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.pointsThre > 0)) + { + gMmwMssMCB.presenceDetEnabled = true; + } + + /* Boundary Box configuration */ + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.occupancyBox[0].x1= (float) atof (argv[1]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.occupancyBox[0].x2= (float) atof (argv[2]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.occupancyBox[0].y1= (float) atof (argv[3]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.occupancyBox[0].y2= (float) atof (argv[4]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.occupancyBox[0].z1= (float) atof (argv[5]); + gMmwMssMCB.trackerCfg.trackerDpuCfg.staticCfg.presenceParams.occupancyBox[0].z2= (float) atof (argv[6]); + + return 0; + +} + diff --git a/mss/tracker_utils.h b/mss/tracker_utils.h new file mode 100644 index 0000000..5ad09b5 --- /dev/null +++ b/mss/tracker_utils.h @@ -0,0 +1,212 @@ +/** + * @file task_mbox.c + * + * @brief + * MSS main implementation of the millimeter wave Demo + * + * Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/************************************************************************** + *************************** Include Files ******************************** + **************************************************************************/ + +/* Standard Include Files. */ +#include +#include +#include +#include +#include +#include + +/************************************************************************** + *************************** Local Definitions **************************** + **************************************************************************/ + +/************************************************************************** + *************************** Global Definitions *************************** + **************************************************************************/ + +void *gtrack_alloc(unsigned int numElements, unsigned int sizeInBytes); +void gtrack_free(void *pFree, unsigned int sizeInBytes); + +/** + * @b Description + * @n + * This is the CLI Handler for tracking configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ + +int32_t MmwDemo_CLITrackingCfg (int32_t argc, char* argv[]); +/** + * @b Description + * @n + * This is the CLI Handler for static boundary box configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIStaticBoundaryBoxCfg (int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for boundary box configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIBoundaryBoxCfg (int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for sensor position and orientation configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLISensorPositionCfg (int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for GatingParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIGatingParamCfg (int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for StateParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIStateParamCfg (int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for AllocationParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIAllocationParamCfg (int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for tracker Acceleration configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemoCLIMaxAccelerationParamCfg(int32_t argc, char* argv[]); + +/** + * @b Description + * @n + * This is the CLI Handler for PresenceParam configuration + * + * @param[in] argc + * Number of arguments + * @param[in] argv + * Arguments + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t MmwDemo_CLIPresenceParamCfg (int32_t argc, char* argv[]); diff --git a/objdetrangehwa.c b/objdetrangehwa.c new file mode 100644 index 0000000..f4037ad --- /dev/null +++ b/objdetrangehwa.c @@ -0,0 +1,1209 @@ +/* + * @file objdetrangehwa.c + * + * @brief + * Object Detection DPC implementation with range HWA DPU only + * + * \par + * NOTE: + * (C) Copyright 2019 Texas Instruments, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/************************************************************************** + *************************** Include Files ******************************** + **************************************************************************/ +#include +#include +#include + +#define DBG_DPC_OBJDETRANGEHWA + +/* mmWave SDK Include Files: */ +#include +#include +#include +#include +#include +#include +#include +#include + +/* DPUs */ +#include + + /** @addtogroup DPC_OBJDETRANGEHWA_IOCTL__INTERNAL_DEFINITIONS + @{ */ + +/*! This is supplied at command line when application builds this file. This file + * is owned by the application and contains all resource partitioning, an + * application may include more than one DPC and also use resources outside of DPCs. + * The resource definitions used by this object detection DPC are prefixed by DPC_OBJDETRANGEHWA */ +#include APP_RESOURCE_FILE + +/* Obj Det instance etc */ +#include +#include + +#ifdef DBG_DPC_OBJDETRANGEHWA +ObjDetObj *gObjDetObj; +#endif + +/*! Radar cube data buffer alignment in bytes. No DPU module specifying alignment + * need (through a \#define) implies that no alignment is needed i.e 1 byte alignment. + * But we do the natural data type alignment which is 2 bytes (as radar cube is complex16-bit type) + * because radar cube is exported out of DPC in processing result so assume CPU may access + * it for post-DPC processing. + */ +#define DPC_OBJDETRANGEHWA_RADAR_CUBE_DATABUF_BYTE_ALIGNMENT (sizeof(int16_t)) + + +/** +@} +*/ + +#define DPC_OBJDETRANGEHWA_HWA_MAX_WINDOW_RAM_SIZE_IN_SAMPLES SOC_HWA_WINDOW_RAM_SIZE_IN_SAMPLES + +/******************************************************************************/ +/* Local definitions */ +#define DPC_USE_SYMMETRIC_WINDOW_RANGE_DPU +#define DPC_DPU_RANGEPROC_FFT_WINDOW_TYPE MATHUTILS_WIN_BLACKMAN + + +#define DPC_OBJDET_QFORMAT_RANGE_FFT 17 + +/************************************************************************** + ************************** Local Functions ******************************* + **************************************************************************/ + +static DPM_DPCHandle DPC_ObjectDetection_init +( + DPM_Handle dpmHandle, + DPM_InitCfg* ptrInitCfg, + int32_t* errCode +); + +static int32_t DPC_ObjectDetection_execute +( + DPM_DPCHandle handle, + DPM_Buffer* ptrResult +); + +static int32_t DPC_ObjectDetection_ioctl +( + DPM_DPCHandle handle, + uint32_t cmd, + void* arg, + uint32_t argLen +); + +static int32_t DPC_ObjectDetection_start (DPM_DPCHandle handle); +static int32_t DPC_ObjectDetection_stop (DPM_DPCHandle handle); +static int32_t DPC_ObjectDetection_deinit (DPM_DPCHandle handle); +static void DPC_ObjectDetection_frameStart (DPM_DPCHandle handle); + +/************************************************************************** + ************************* Global Declarations **************************** + **************************************************************************/ + +/** @addtogroup DPC_OBJDETRANGEHWA__GLOBAL + @{ */ + +/** + * @brief Global used to register Object Detection DPC in DPM + */ +DPM_ProcChainCfg gDPC_ObjDetRangeHWACfg = +{ + DPC_ObjectDetection_init, /* Initialization Function: */ + DPC_ObjectDetection_start, /* Start Function: */ + DPC_ObjectDetection_execute, /* Execute Function: */ + DPC_ObjectDetection_ioctl, /* Configuration Function: */ + DPC_ObjectDetection_stop, /* Stop Function: */ + DPC_ObjectDetection_deinit, /* Deinitialization Function: */ + NULL, /* Inject Data Function: */ + NULL, /* Chirp Available Function: */ + DPC_ObjectDetection_frameStart /* Frame Start Function: */ +}; + +/* @} */ + +/** + * @b Description + * @n + * Utility function for reseting memory pool. + * + * @param[in] pool Handle to pool object. + * + * \ingroup DPC_OBJDETRANGEHWA_INTERNAL_FUNCTION + * + * @retval + * none. + */ +static void DPC_ObjDetRangeHwa_MemPoolReset(MemPoolObj *pool) +{ + pool->currAddr = (uintptr_t)pool->cfg.addr; + pool->maxCurrAddr = pool->currAddr; +} + + +/** + * @b Description + * @n + * Utility function for getting maximum memory pool usage. + * + * @param[in] pool Handle to pool object. + * + * \ingroup DPC_OBJDETRANGEHWA_INTERNAL_FUNCTION + * + * @retval + * Amount of pool used in bytes. + */ +static uint32_t DPC_ObjDetRangeHwa_MemPoolGetMaxUsage(MemPoolObj *pool) +{ + return((uint32_t)(pool->maxCurrAddr - (uintptr_t)pool->cfg.addr)); +} + +/** + * @b Description + * @n + * Utility function for allocating from a static memory pool. + * + * @param[in] pool Handle to pool object. + * @param[in] size Size in bytes to be allocated. + * @param[in] align Alignment in bytes + * + * \ingroup DPC_OBJDETRANGEHWA_INTERNAL_FUNCTION + * + * @retval + * pointer to beginning of allocated block. NULL indicates could not + * allocate. + */ +static void *DPC_ObjDetRangeHwa_MemPoolAlloc(MemPoolObj *pool, + uint32_t size, + uint8_t align) +{ + void *retAddr = NULL; + uintptr_t addr; + + addr = MEM_ALIGN(pool->currAddr, align); + if ((addr + size) <= ((uintptr_t)pool->cfg.addr + pool->cfg.size)) + { + retAddr = (void *)addr; + pool->currAddr = addr + size; + pool->maxCurrAddr = MAX(pool->currAddr, pool->maxCurrAddr); + } + + return(retAddr); +} + + +/** + * @b Description + * @n + * Sends Assert + * + * @retval + * Not Applicable. + */ +void _DPC_Objdet_Assert(DPM_Handle handle, int32_t expression, + const char *file, int32_t line) +{ + DPM_DPCAssert fault; + + if (!expression) + { + fault.lineNum = (uint32_t)line; + fault.arg0 = 0U; + fault.arg1 = 0U; + strncpy (fault.fileName, file, (DPM_MAX_FILE_NAME_LEN-1)); + + /* Report the fault to the DPM entities */ + DPM_ioctl (handle, + DPM_CMD_DPC_ASSERT, + (void*)&fault, + sizeof(DPM_DPCAssert)); + } +} + + +/** + * @b Description + * @n + * Computes the length of window to generate for range DPU. + * + * @param[in] numAdcSamples Number of ADC samples + * + * @retval Length of window to generate + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + */ +static uint32_t DPC_ObjDetRangeHwa_GetRangeWinGenLen(uint16_t numAdcSamples) +{ + uint32_t winGenLen; + +#ifdef DPC_USE_SYMMETRIC_WINDOW_RANGE_DPU + winGenLen = (numAdcSamples + 1)/2; +#else + winGenLen = numAdcSamples; +#endif + return(winGenLen); +} + +/** + * @b Description + * @n + * Generate the range DPU window using mathutils API. + * + * @param[in] cfg Range DPU configuration, output window is generated in window + * pointer in the staticCfg of this. + * + * @retval None + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + */ +static void DPC_ObjDetRangeHwa_GenRangeWindow(DPU_RangeProcHWA_Config *cfg) +{ + mathUtils_genWindow((uint32_t *)cfg->staticCfg.window, + cfg->staticCfg.ADCBufData.dataProperty.numAdcSamples, + DPC_ObjDetRangeHwa_GetRangeWinGenLen(cfg->staticCfg.ADCBufData.dataProperty.numAdcSamples), + DPC_DPU_RANGEPROC_FFT_WINDOW_TYPE, + DPC_OBJDET_QFORMAT_RANGE_FFT); +} + + +/** + * @b Description + * @n + * Sub-frame reconfiguration, used when switching sub-frames. Invokes the + * DPU configuration using the configuration that was stored during the + * pre-start configuration so reconstruction time is saved because this will + * happen in real-time. + * @param[in] objDetObj Pointer to DPC object + * @param[in] subFrameIndx Sub-frame index. + * + * @retval + * Success - 0 + * @retval + * Error - <0 + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + */ +static int32_t DPC_ObjDetRangeHwa_reconfigSubFrame(ObjDetObj *objDetObj, uint8_t subFrameIndx) +{ + int32_t retVal = 0; + SubFrameObj *subFrmObj; + + subFrmObj = &objDetObj->subFrameObj[subFrameIndx]; + + if(objDetObj->commonCfg.numSubFrames > 1) + { + DPC_ObjDetRangeHwa_GenRangeWindow(&subFrmObj->rangeCfg); + retVal = DPU_RangeProcHWA_config(subFrmObj->dpuRangeObj, &subFrmObj->rangeCfg); + if (retVal != 0) + { + goto exit; + } + } +exit: + return(retVal); +} + +/** + * @b Description + * @n + * Configure range DPU. + * + * @param[in] dpuHandle Handle to DPU + * @param[in] staticCfg Pointer to static configuration of the sub-frame + * @param[in] dynCfg Pointer to dynamic configuration of the sub-frame + * @param[in] edmaHandle Handle to edma driver to be used for the DPU + * @param[in] radarCube Pointer to DPIF radar cube, which is output of range + * processing. + * @param[in] CoreLocalRamObj Pointer to core local RAM object to allocate local memory + * for the DPU, only for scratch purposes + * @param[out] cfgSave Configuration that is built in local + * (stack) variable is saved here. This is for facilitating + * quick reconfiguration later without having to go through + * the construction of the configuration. + * + * @retval + * Success - 0 + * @retval + * Error - <0 + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + */ +static int32_t DPC_ObjDetRangeHwa_rangeConfig +( + DPU_RangeProcHWA_Handle dpuHandle, + DPC_ObjectDetectionRangeHWA_StaticCfg *staticCfg, + DPC_ObjectDetectionRangeHWA_DynCfg *dynCfg, + EDMA_Handle edmaHandle, + DPIF_RadarCube *radarCube, + MemPoolObj *CoreLocalRamObj, + DPU_RangeProcHWA_Config *cfgSave +) +{ + int32_t retVal = 0; + DPU_RangeProcHWA_Config rangeCfg; + DPU_RangeProcHWA_HW_Resources *hwRes = &rangeCfg.hwRes; + DPU_RangeProcHWA_EDMAInputConfig *edmaIn = &hwRes->edmaInCfg; + DPU_RangeProcHWA_EDMAOutputConfig *edmaOut = &hwRes->edmaOutCfg; + DPU_RangeProcHWA_HwaConfig *hwaCfg = &hwRes->hwaCfg; + uint32_t numRxAntennas, winGenLen; + + memset(&rangeCfg, 0, sizeof(rangeCfg)); + + numRxAntennas = staticCfg->ADCBufData.dataProperty.numRxAntennas; + + /* Even though Range DPU supports both modes, + * object detection DPC only supports non-interleaved at present */ + DebugP_assert(staticCfg->ADCBufData.dataProperty.interleave == DPIF_RXCHAN_NON_INTERLEAVE_MODE); + + /* dynamic configuration */ + rangeCfg.dynCfg.calibDcRangeSigCfg = &dynCfg->calibDcRangeSigCfg; + + /* static configuration */ + rangeCfg.staticCfg.ADCBufData = staticCfg->ADCBufData; + rangeCfg.staticCfg.numChirpsPerFrame = staticCfg->numChirpsPerFrame; + rangeCfg.staticCfg.numRangeBins = staticCfg->numRangeBins; + rangeCfg.staticCfg.numTxAntennas = staticCfg->numTxAntennas; + rangeCfg.staticCfg.numVirtualAntennas = staticCfg->numVirtualAntennas; + rangeCfg.staticCfg.resetDcRangeSigMeanBuffer = 1; + rangeCfg.staticCfg.rangeFFTtuning.fftOutputDivShift = staticCfg->rangeFFTtuning.fftOutputDivShift; + rangeCfg.staticCfg.rangeFFTtuning.numLastButterflyStagesToScale = staticCfg->rangeFFTtuning.numLastButterflyStagesToScale; + + /* radarCube */ + rangeCfg.hwRes.radarCube = *radarCube; + + /* static configuration - window */ + /* Generating 1D window, allocate first */ + winGenLen = DPC_ObjDetRangeHwa_GetRangeWinGenLen(rangeCfg.staticCfg.ADCBufData.dataProperty.numAdcSamples); + rangeCfg.staticCfg.windowSize = winGenLen * sizeof(uint32_t); + rangeCfg.staticCfg.window = (int32_t *)DPC_ObjDetRangeHwa_MemPoolAlloc(CoreLocalRamObj, rangeCfg.staticCfg.windowSize, sizeof(uint32_t)); + if (rangeCfg.staticCfg.window == NULL) + { + retVal = DPC_OBJDETRANGEHWA_ENOMEM__CORE_LOCAL_RAM_RANGE_HWA_WINDOW; + goto exit; + } + DPC_ObjDetRangeHwa_GenRangeWindow(&rangeCfg); + + /* hwres - dcRangeSig, allocate from heap, this needs to persist within sub-frame/frame + * processing and across sub-frames */ + hwRes->dcRangeSigMeanSize = DPU_RANGEPROC_SIGNATURE_COMP_MAX_BIN_SIZE * + staticCfg->numTxAntennas * numRxAntennas * sizeof(cmplx32ImRe_t); + hwRes->dcRangeSigMean = (cmplx32ImRe_t *) MemoryP_ctrlAlloc (hwRes->dcRangeSigMeanSize, + (uint8_t) 0 /*sizeof(cmplx32ImRe_t)*/); + DebugP_assert(rangeCfg.hwRes.dcRangeSigMeanSize == hwRes->dcRangeSigMeanSize); + + /* hwres - edma */ + hwRes->edmaHandle = edmaHandle; + /* We have choosen ISOLATE mode, so we have to fill in dataIn */ + edmaIn->dataIn.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_CH; + edmaIn->dataIn.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SHADOW; + edmaIn->dataIn.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_EVENT_QUE; + edmaIn->dataInSignature.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SIG_CH; + edmaIn->dataInSignature.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SIG_SHADOW; + edmaIn->dataInSignature.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SIG_EVENT_QUE; + + /* We are radar Cube FORMAT1 and non-interleaved ADC, so for 3 tx antenna case, we have to + * fill format2, otherwise format1 + */ + if ((staticCfg->numTxAntennas == 3) && (radarCube->datafmt == DPIF_RADARCUBE_FORMAT_1)) + { + /* Ping */ + /* Ping - dataOutPing */ + edmaOut->u.fmt2.dataOutPing.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PING_CH; + edmaOut->u.fmt2.dataOutPing.channelShadow[0] = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PING_SHADOW_0; + edmaOut->u.fmt2.dataOutPing.channelShadow[1] = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PING_SHADOW_1; + edmaOut->u.fmt2.dataOutPing.channelShadow[2] = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PING_SHADOW_2; + edmaOut->u.fmt2.dataOutPing.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PING_EVENT_QUE; + /* Ping - dataOutPingData */ + edmaOut->u.fmt2.dataOutPingData[0].channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_0_CH; + edmaOut->u.fmt2.dataOutPingData[0].channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_0_SHADOW; + edmaOut->u.fmt2.dataOutPingData[0].eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_0_EVENT_QUE; + edmaOut->u.fmt2.dataOutPingData[1].channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_1_CH; + edmaOut->u.fmt2.dataOutPingData[1].channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_1_SHADOW; + edmaOut->u.fmt2.dataOutPingData[1].eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_1_EVENT_QUE; + edmaOut->u.fmt2.dataOutPingData[2].channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_2_CH; + edmaOut->u.fmt2.dataOutPingData[2].channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_2_SHADOW; + edmaOut->u.fmt2.dataOutPingData[2].eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PINGDATA_2_EVENT_QUE; + + /* Pong */ + /* Pong - dataOutPong */ + edmaOut->u.fmt2.dataOutPong.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONG_CH; + edmaOut->u.fmt2.dataOutPong.channelShadow[0] = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONG_SHADOW_0; + edmaOut->u.fmt2.dataOutPong.channelShadow[1] = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONG_SHADOW_1; + edmaOut->u.fmt2.dataOutPong.channelShadow[2] = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONG_SHADOW_2; + edmaOut->u.fmt2.dataOutPong.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONG_EVENT_QUE; + /* Pong - dataOutPongData */ + edmaOut->u.fmt2.dataOutPongData[0].channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_0_CH; + edmaOut->u.fmt2.dataOutPongData[0].channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_0_SHADOW; + edmaOut->u.fmt2.dataOutPongData[0].eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_0_EVENT_QUE; + edmaOut->u.fmt2.dataOutPongData[1].channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_1_CH; + edmaOut->u.fmt2.dataOutPongData[1].channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_1_SHADOW; + edmaOut->u.fmt2.dataOutPongData[1].eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_1_EVENT_QUE; + edmaOut->u.fmt2.dataOutPongData[2].channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_2_CH; + edmaOut->u.fmt2.dataOutPongData[2].channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_2_SHADOW; + edmaOut->u.fmt2.dataOutPongData[2].eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT2_PONGDATA_2_EVENT_QUE; + } + else + { + /* Ping */ + edmaOut->u.fmt1.dataOutPing.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT1_PING_CH; + edmaOut->u.fmt1.dataOutPing.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT1_PING_SHADOW; + edmaOut->u.fmt1.dataOutPing.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT1_PING_EVENT_QUE; + + /* Pong */ + edmaOut->u.fmt1.dataOutPong.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT1_PONG_CH; + edmaOut->u.fmt1.dataOutPong.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT1_PONG_SHADOW; + edmaOut->u.fmt1.dataOutPong.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_FMT1_PONG_EVENT_QUE; + } + + edmaOut->dataOutSignature.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_SIG_CH; + edmaOut->dataOutSignature.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_SIG_SHADOW; + edmaOut->dataOutSignature.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_SIG_EVENT_QUE; + + /* hwres - hwa */ + /* Use ISOLATED mode to support CBUFF in future */ + hwaCfg->dataInputMode = DPU_RangeProcHWA_InputMode_ISOLATED; + +#ifdef DPC_USE_SYMMETRIC_WINDOW_RANGE_DPU + hwaCfg->hwaWinSym = HWA_FFT_WINDOW_SYMMETRIC; +#else + hwaCfg->hwaWinSym = HWA_FFT_WINDOW_NONSYMMETRIC; +#endif + hwaCfg->hwaWinRamOffset = (uint16_t) DPC_OBJDET_HWA_WINDOW_RAM_OFFSET; + if ((hwaCfg->hwaWinRamOffset + winGenLen) > DPC_OBJDETRANGEHWA_HWA_MAX_WINDOW_RAM_SIZE_IN_SAMPLES) + { + retVal = DPC_OBJDETRANGEHWA_ENOMEM_HWA_WINDOW_RAM; + goto exit; + } + + hwaCfg->numParamSet = DPU_RANGEPROCHWA_NUM_HWA_PARAM_SETS; + hwaCfg->paramSetStartIdx = DPC_OBJDET_DPU_RANGEPROC_PARAMSET_START_IDX; + + *cfgSave = rangeCfg; + + retVal = DPU_RangeProcHWA_config(dpuHandle, &rangeCfg); + if (retVal != 0) + { + goto exit; + } + + /* store configuration for use in intra-sub-frame processing and + * inter-sub-frame switching, although window will need to be regenerated and + * dc range sig should not be reset. */ + rangeCfg.staticCfg.resetDcRangeSigMeanBuffer = 0; + *cfgSave = rangeCfg; + +exit: + return retVal; +} + +/** + * @b Description + * @n + * Performs processing related to pre-start configuration, which is per sub-frame, + * by configuring each of the DPUs involved in the processing chain. + * Memory management notes: + * 1. Core Local Memory that needs to be preserved across sub-frames (such as range DPU's calib DC buffer) + * will be allocated using MemoryP_alloc. + * 2. Core Local Memory that needs to be preserved within a sub-frame across DPU calls + * (the DPIF * type memory) or for intermediate private scratch memory for + * DPU (i.e no preservation is required from process call to process call of the DPUs + * within the sub-frame) will be allocated from the Core Local RAM configuration supplied in + * @ref DPC_ObjectDetectionRangeHWA_InitParams given to @ref DPC_ObjectDetection_init API + * 3. L3 memory will only be allocated from the L3 RAM configuration supplied in + * @ref DPC_ObjectDetectionRangeHWA_InitParams given to @ref DPC_ObjectDetection_init API + * No L3 buffers are presently required that need to be preserved across sub-frames + * (type described in #1 above), neither are L3 scratch buffers required for + * intermediate processing within DPU process call. + * + * @param[in] subFrmObj Pointer to sub-frame object + * @param[in] objDetObj Pointer to objDetObj + * @param[in] staticCfg Pointer to static configuration of the sub-frame + * @param[in] dynCfg Pointer to dynamic configuration of the sub-frame + * @param[out] memUsage Pointer to memory usage structure to report mmemory + usage in this DPC + * + * @retval + * Success - 0 + * @retval + * Error - <0 + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + */ +static int32_t DPC_ObjDetRangeHwa_preStartConfig +( + SubFrameObj *subFrmObj, + ObjDetObj *objDetObj, + DPC_ObjectDetectionRangeHWA_StaticCfg *staticCfg, + DPC_ObjectDetectionRangeHWA_DynCfg *dynCfg, + DPC_ObjectDetectionRangeHWA_preStartCfg_memUsage *memUsage +) +{ + int32_t retVal = 0; + DPIF_RadarCube radarCube; + MemPoolObj *L3RamObj; + MemPoolObj *CoreLocalRamObj; + + L3RamObj = &objDetObj->L3RamObj; + CoreLocalRamObj = &objDetObj->CoreLocalRamObj; + + DPC_ObjDetRangeHwa_MemPoolReset(L3RamObj); + DPC_ObjDetRangeHwa_MemPoolReset(CoreLocalRamObj); + + /* L3 allocations */ + /* L3 - radar cube -- This has be synced with the DSP chain */ + radarCube.dataSize = staticCfg->numRangeBins * staticCfg->numDopplerChirps * + staticCfg->numVirtualAntennas * sizeof(cmplx16ReIm_t); + radarCube.data = DPC_ObjDetRangeHwa_MemPoolAlloc(L3RamObj, radarCube.dataSize, + DPC_OBJDETRANGEHWA_RADAR_CUBE_DATABUF_BYTE_ALIGNMENT); + if (radarCube.data == NULL) + { + retVal = DPC_OBJDETRANGEHWA_ENOMEM__L3_RAM_RADAR_CUBE; + goto exit; + } + radarCube.datafmt = staticCfg->radarCubeFormat; + + retVal = DPC_ObjDetRangeHwa_rangeConfig(subFrmObj->dpuRangeObj, + staticCfg, + dynCfg, + objDetObj->edmaHandle, + &radarCube, CoreLocalRamObj, + &subFrmObj->rangeCfg); + if (retVal != 0) + { + goto exit; + } + + /* Report RAM usage */ + memUsage->CoreLocalRamUsage = DPC_ObjDetRangeHwa_MemPoolGetMaxUsage(CoreLocalRamObj); + memUsage->L3RamUsage = DPC_ObjDetRangeHwa_MemPoolGetMaxUsage(L3RamObj); + +exit: + return retVal; +} + + +/** + * @b Description + * @n + * DPC frame start function registered with DPM. This is invoked on reception + * of the frame start ISR from the RF front-end. + * + * @param[in] handle DPM's DPC handle + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Not applicable + */ +static void DPC_ObjectDetection_frameStart (DPM_DPCHandle handle) +{ + ObjDetObj *objDetObj = (ObjDetObj *) handle; + + objDetObj->stats.frameStartTimeStamp = Cycleprofiler_getTimeStamp(); + + DebugP_log2("ObjDet DPC: Frame Start, frameIndx = %d, subFrameIndx = %d\n", + objDetObj->stats.frameStartIntCounter, objDetObj->subFrameIndx); + + /* Check if previous frame (sub-frame) processing has completed */ + DPC_Objdet_Assert(objDetObj->dpmHandle, (objDetObj->interSubFrameProcToken == 0)); + objDetObj->interSubFrameProcToken++; + + /* Increment interrupt counter for debugging purpose */ + if (objDetObj->subFrameIndx == 0) + { + objDetObj->stats.frameStartIntCounter++; + } + + /* Notify the DPM Module that the DPC is ready for execution */ + DebugP_assert (DPM_notifyExecute (objDetObj->dpmHandle, handle, true) == 0); + return; +} + +/** + * @b Description + * @n + * DPC's (DPM registered) execute function which is invoked by the application + * in the DPM's execute context when the DPC issues DPM_notifyExecute API from + * its registered @ref DPC_ObjectDetection_frameStart API that is invoked every + * frame interrupt. + * + * @param[in] handle DPM's DPC handle + * @param[out] ptrResult Pointer to the result + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +int32_t DPC_ObjectDetection_execute +( + DPM_DPCHandle handle, + DPM_Buffer* ptrResult +) +{ + ObjDetObj *objDetObj; + SubFrameObj *subFrmObj; + DPU_RangeProcHWA_OutParams outRangeProc; + int32_t retVal; + DPM_Buffer rangeProcResult; + + objDetObj = (ObjDetObj *) handle; + DebugP_assert (objDetObj != NULL); + DebugP_assert (ptrResult != NULL); + + DebugP_log1("ObjDet DPC: Processing sub-frame %d\n", objDetObj->subFrameIndx); + + subFrmObj = &objDetObj->subFrameObj[objDetObj->subFrameIndx]; + if (objDetObj->processCallBackFxn.processFrameBeginCallBackFxn != NULL) + { + (*objDetObj->processCallBackFxn.processFrameBeginCallBackFxn)(objDetObj->subFrameIndx); + } + + retVal = DPU_RangeProcHWA_process(subFrmObj->dpuRangeObj, &outRangeProc); + if (retVal != 0) + { + goto exit; + } + DebugP_assert(outRangeProc.endOfChirp == true); + + subFrmObj = &objDetObj->subFrameObj[objDetObj->subFrameIndx]; + if (objDetObj->processCallBackFxn.processInterFrameBeginCallBackFxn != NULL) + { + (*objDetObj->processCallBackFxn.processInterFrameBeginCallBackFxn)(objDetObj->subFrameIndx); + } + + /******************************************************************************** + * Range Processing is finished, now it can send data to inter frame processing DPC. + ********************************************************************************/ + rangeProcResult.ptrBuffer[0] = objDetObj->subFrameObj[objDetObj->subFrameIndx].rangeCfg.hwRes.radarCube.data; + rangeProcResult.size[0] = objDetObj->subFrameObj[objDetObj->subFrameIndx].rangeCfg.hwRes.radarCube.dataSize; + + /* Relay the results: */ + retVal = DPM_relayResult(objDetObj->dpmHandle, handle, &rangeProcResult); + DebugP_assert (retVal == 0); + + /******************************************************************************** + * This DPC does not report results to the application so for the + * sake of clarity reset the result buffer + ********************************************************************************/ + memset ((void *)ptrResult, 0, sizeof(DPM_Buffer)); + objDetObj->interSubFrameProcToken--; + + /******************************************************************************** + * Prepare for next Frame + ********************************************************************************/ + + /* Update subFrame index */ + objDetObj->subFrameIndx++; + objDetObj->subFrameIndx = objDetObj->subFrameIndx % objDetObj->commonCfg.numSubFrames; + + if(objDetObj->commonCfg.numSubFrames > 1U) + { + /* Re-configure Range DPU for next subFrame */ + retVal = DPC_ObjDetRangeHwa_reconfigSubFrame(objDetObj, objDetObj->subFrameIndx); + DebugP_assert (retVal == 0); + } + + /* Trigger Range DPU for next frame */ + retVal = DPU_RangeProcHWA_control(objDetObj->subFrameObj[objDetObj->subFrameIndx].dpuRangeObj, + DPU_RangeProcHWA_Cmd_triggerProc, NULL, 0); + DPC_Objdet_Assert(objDetObj->dpmHandle, (retVal == 0)); + + /* For rangeProcHwa, interChirpProcessingMargin is not available */ + objDetObj->stats.interChirpProcessingMargin = 0; + objDetObj->stats.interFrameStartTimeStamp = Cycleprofiler_getTimeStamp(); + + DebugP_log0("ObjDet DPC: Range Proc Done\n"); + +exit: + return retVal; +} + +/** + * @b Description + * @n + * DPC's (DPM registered) start function which is invoked by the + * application using DPM_start API. + * + * @param[in] handle DPM's DPC handle + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t DPC_ObjectDetection_start (DPM_DPCHandle handle) +{ + ObjDetObj *objDetObj; + SubFrameObj *subFrmObj; + int32_t retVal = 0; + + objDetObj = (ObjDetObj *) handle; + DebugP_assert (objDetObj != NULL); + + objDetObj->stats.frameStartIntCounter = 0; + + /* Start marks consumption of all pre-start configs, reset the flag to check + * if pre-starts were issued only after common config was issued for the next + * time full configuration happens between stop and start */ + objDetObj->isCommonCfgReceived = false; + + /* App must issue export of last frame after stop which will switch to sub-frame 0, + * so start should always see sub-frame indx of 0, check */ + DebugP_assert(objDetObj->subFrameIndx == 0); + + if(objDetObj->commonCfg.numSubFrames > 1U) + { + /* Pre-start cfgs for sub-frames may have come in any order, so need + * to ensure we reconfig for the current (0) sub-frame before starting */ + DPC_ObjDetRangeHwa_reconfigSubFrame(objDetObj, objDetObj->subFrameIndx); + } + + /* Trigger Range DPU, related to reconfig above */ + subFrmObj = &objDetObj->subFrameObj[objDetObj->subFrameIndx]; + retVal = DPU_RangeProcHWA_control(subFrmObj->dpuRangeObj, + DPU_RangeProcHWA_Cmd_triggerProc, NULL, 0); + if(retVal < 0) + { + goto exit; + } + + DebugP_log0("ObjDet DPC: Start done\n"); +exit: + return(retVal); +} + + +/** + * @b Description + * @n + * DPC's (DPM registered) stop function which is invoked by the + * application using DPM_stop API. + * + * @param[in] handle DPM's DPC handle + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t DPC_ObjectDetection_stop (DPM_DPCHandle handle) +{ + ObjDetObj *objDetObj; + + objDetObj = (ObjDetObj *) handle; + DebugP_assert (objDetObj != NULL); + + /* We can be here only after complete frame processing is done, which means + * processing token must be 0 and subFrameIndx also 0 */ + DebugP_assert(objDetObj->subFrameIndx == 0); + + DebugP_log0("ObjDet DPC: Stop done\n"); + return(0); +} + +/** + * @b Description + * @n + * DPC IOCTL commands configuration API which will be invoked by the + * application using DPM_ioctl API + * + * @param[in] handle DPM's DPC handle + * @param[in] cmd Capture DPC specific commands + * @param[in] arg Command specific arguments + * @param[in] argLen Length of the arguments which is also command specific + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t DPC_ObjectDetection_ioctl +( + DPM_DPCHandle handle, + uint32_t cmd, + void* arg, + uint32_t argLen +) +{ + ObjDetObj *objDetObj; + SubFrameObj *subFrmObj; + int32_t retVal = 0; + + /* Get the DSS MCB: */ + objDetObj = (ObjDetObj *) handle; + DebugP_assert(objDetObj != NULL); + DebugP_assert(arg != NULL); + + /* Process the commands. Process non sub-frame specific ones first + * so the sub-frame specific ones can share some code. */ + if((cmd < DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG) || + (cmd > DPC_OBJDETRANGEHWA_IOCTL__STATIC_TRACKER_CFG)) + { + retVal = DPM_EINVCMD; + } + else if (cmd == DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_COMMON_CFG) + { + uint8_t index; + DPC_ObjectDetectionRangeHWA_PreStartCommonCfg *cfg; + + DebugP_assert(argLen == sizeof(DPC_ObjectDetectionRangeHWA_PreStartCommonCfg)); + + cfg = (DPC_ObjectDetectionRangeHWA_PreStartCommonCfg*)arg; + + /* Release DPU DC range Signal mean memory */ + for(index = 0; index < objDetObj->commonCfg.numSubFrames; index ++) + { + subFrmObj = &objDetObj->subFrameObj[index]; + + if (subFrmObj->rangeCfg.hwRes.dcRangeSigMean) + { + MemoryP_ctrlFree(subFrmObj->rangeCfg.hwRes.dcRangeSigMean, + subFrmObj->rangeCfg.hwRes.dcRangeSigMeanSize); + } + } + + objDetObj->commonCfg = *cfg; + objDetObj->isCommonCfgReceived = true; + + DebugP_log0("ObjDet DPC: Pre-start Common Config IOCTL processed\n"); + } +#ifdef TRACKERPROC_EN + else if (cmd == DPC_OBJDETRANGEHWA_IOCTL__STATIC_TRACKER_CFG) + { + //System_printf("Entering Tracker Config"); + DPC_ObjectDetection_TrackerConfig *cfg; + + DebugP_assert(argLen == sizeof(DPC_ObjectDetection_TrackerConfig)); + + cfg = (DPC_ObjectDetection_TrackerConfig*)arg; + + retVal = DPU_TrackerProc_config(objDetObj->dpuTrackerObj, &(cfg->trackerDpuCfg)); + if (retVal != 0) + { + goto exit; + } + } +#endif + else + { + uint8_t subFrameNum; + subFrameNum = *(uint8_t *)arg; + + switch (cmd) + { + /* Range DPU related */ + case DPC_OBJDETRANGEHWA_IOCTL__DYNAMIC_CALIB_DC_RANGE_SIG_CFG: + { + DPC_ObjectDetectionRangeHWA_CalibDcRangeSigCfg *cfg; + + DebugP_assert(argLen == sizeof(DPC_ObjectDetectionRangeHWA_CalibDcRangeSigCfg)); + + cfg = (DPC_ObjectDetectionRangeHWA_CalibDcRangeSigCfg*)arg; + + subFrmObj = &objDetObj->subFrameObj[subFrameNum]; + + retVal = DPU_RangeProcHWA_control(subFrmObj->dpuRangeObj, + DPU_RangeProcHWA_Cmd_dcRangeCfg, + &cfg->cfg, + sizeof(DPU_RangeProc_CalibDcRangeSigCfg)); + if (retVal != 0) + { + goto exit; + } + + /* save into object */ + subFrmObj->dynCfg.calibDcRangeSigCfg = cfg->cfg; + + break; + } + + /* Related to pre-start configuration */ + case DPC_OBJDETRANGEHWA_IOCTL__STATIC_PRE_START_CFG: + { + DPC_ObjectDetectionRangeHWA_PreStartCfg *cfg; + DPC_ObjectDetectionRangeHWA_preStartCfg_memUsage *memUsage; + MemoryP_Stats statsStart; + MemoryP_Stats statsEnd; + + /* Pre-start common config must be received before pre-start configs are received. */ + if (objDetObj->isCommonCfgReceived == false) + { + retVal = DPC_OBJDETRANGEHWA_PRE_START_CONFIG_BEFORE_PRE_START_COMMON_CONFIG; + goto exit; + } + + DebugP_assert(argLen == sizeof(DPC_ObjectDetectionRangeHWA_PreStartCfg)); + + /* Get system heap size before preStart configuration */ + MemoryP_getStats(&statsStart); + + cfg = (DPC_ObjectDetectionRangeHWA_PreStartCfg*)arg; + subFrmObj = &objDetObj->subFrameObj[subFrameNum]; + + memUsage = &cfg->memUsage; + memUsage->L3RamTotal = objDetObj->L3RamObj.cfg.size; + memUsage->CoreLocalRamTotal = objDetObj->CoreLocalRamObj.cfg.size; + retVal = DPC_ObjDetRangeHwa_preStartConfig(subFrmObj, + objDetObj, + &cfg->staticCfg, + &cfg->dynCfg, + memUsage); + if (retVal != 0) + { + goto exit; + } + + /* Populate radarCube memory information */ + cfg->radarCubeMem.addr = subFrmObj->rangeCfg.hwRes.radarCube.data; + cfg->radarCubeMem.size = subFrmObj->rangeCfg.hwRes.radarCube.dataSize; + + /* Get system heap size after preStart configuration */ + MemoryP_getStats(&statsEnd); + + /* Populate system heap usage */ + memUsage->SystemHeapTotal = statsEnd.totalSize; + memUsage->SystemHeapUsed = statsEnd.totalSize -statsEnd.totalFreeSize; + memUsage->SystemHeapDPCUsed = statsStart.totalFreeSize - statsEnd.totalFreeSize; + + DebugP_log1("ObjDet DPC: Pre-start Config IOCTL processed (subFrameIndx = %d)\n", cfg->subFrameNum); + break; + } + + default: + { + /* Error: This is an unsupported command */ + retVal = DPM_EINVCMD; + break; + } + } + } + +exit: + return retVal; +} + +/** + * @b Description + * @n + * DPC's (DPM registered) initialization function which is invoked by the + * application using DPM_init API. Among other things, this API allocates DPC instance + * and DPU instances (by calling DPU's init APIs) from the MemoryP osal + * heap. If this API returns an error of any type, the heap is not guaranteed + * to be in the same state as before calling the API (i.e any allocations + * from the heap while executing the API are not guaranteed to be deallocated + * in case of error), so any error from this API should be considered fatal and + * if the error is of _ENOMEM type, the application will + * have to be built again with a bigger heap size to address the problem. + * + * @param[in] dpmHandle DPM's DPC handle + * @param[in] ptrInitCfg Handle to the framework semaphore + * @param[out] errCode Error code populated on error + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static DPM_DPCHandle DPC_ObjectDetection_init +( + DPM_Handle dpmHandle, + DPM_InitCfg* ptrInitCfg, + int32_t* errCode +) +{ + int32_t i; + ObjDetObj *objDetObj = NULL; + DPC_ObjectDetectionRangeHWA_InitParams *dpcInitParams; + HWA_MemInfo hwaMemInfo; + uint8_t index; + + *errCode = 0; + + if ((ptrInitCfg == NULL) || (ptrInitCfg->arg == NULL)) + { + *errCode = DPC_OBJDETRANGEHWA_EINVAL; + goto exit; + } + + if (ptrInitCfg->argSize != sizeof(DPC_ObjectDetectionRangeHWA_InitParams)) + { + *errCode = DPC_OBJDETRANGEHWA_EINVAL__INIT_CFG_ARGSIZE; + goto exit; + } + + dpcInitParams = (DPC_ObjectDetectionRangeHWA_InitParams *) ptrInitCfg->arg; + + objDetObj = MemoryP_ctrlAlloc(sizeof(ObjDetObj), 0); + +#ifdef DBG_DPC_OBJDETRANGEHWA + gObjDetObj = objDetObj; +#endif + + DebugP_log1("ObjDet DPC: objDetObj address = %d\n", (uint32_t) objDetObj); + + if(objDetObj == NULL) + { + *errCode = DPC_OBJDETRANGEHWA_ENOMEM; + goto exit; + } + + /* Initialize memory */ + memset((void *)objDetObj, 0, sizeof(ObjDetObj)); + + /* Copy over the DPM configuration: */ + memcpy ((void*)&objDetObj->dpmInitCfg, (void*)ptrInitCfg, sizeof(DPM_InitCfg)); + + objDetObj->dpmHandle = dpmHandle; + objDetObj->socHandle = ptrInitCfg->socHandle; + objDetObj->L3RamObj.cfg = dpcInitParams->L3ramCfg; + objDetObj->CoreLocalRamObj.cfg = dpcInitParams->CoreLocalRamCfg; + + /* Validate EDMA handle */ + DebugP_assert(dpcInitParams->edmaHandle!= NULL); + objDetObj->edmaHandle = dpcInitParams->edmaHandle; + + /* Validate HWA handle */ + DebugP_assert(dpcInitParams->hwaHandle!= NULL); + objDetObj->hwaHandle = dpcInitParams->hwaHandle; + objDetObj->processCallBackFxn = dpcInitParams->processCallBackFxn; + + /* Set HWA bank memory address */ + *errCode = HWA_getHWAMemInfo(dpcInitParams->hwaHandle, &hwaMemInfo); + if (*errCode != 0) + { + goto exit; + } + + objDetObj->hwaMemBankSize = hwaMemInfo.bankSize; + + for (i = 0; i < hwaMemInfo.numBanks; i++) + { + objDetObj->hwaMemBankAddr[i] = hwaMemInfo.baseAddress + + i * hwaMemInfo.bankSize; + } + + for(index = 0; index < RL_MAX_SUBFRAMES; index++) + { + DPU_RangeProcHWA_InitParams rangeInitParams; + + rangeInitParams.hwaHandle = objDetObj->hwaHandle; + + objDetObj->subFrameObj[index].dpuRangeObj = DPU_RangeProcHWA_init(&rangeInitParams, errCode); + } +#ifdef TRACKERPROC_EN + if (*errCode != 0) + { + goto exit; + } + + /* Initialize the tracker DPU - this is not subframe based */ + objDetObj->dpuTrackerObj = DPU_TrackerProc_init(errCode); + + if (*errCode != 0) + { + goto exit; + } +#endif +exit: + if(*errCode != 0) + { + if(objDetObj != NULL) + { + MemoryP_ctrlFree(objDetObj, sizeof(ObjDetObj)); + objDetObj = NULL; + } + } + + return ((DPM_DPCHandle)objDetObj); +} + +/** + * @b Description + * @n + * DPC's (DPM registered) de-initialization function which is invoked by the + * application using DPM_deinit API. + * + * @param[in] handle DPM's DPC handle + * + * \ingroup DPC_OBJDETRANGEHWA__INTERNAL_FUNCTION + * + * @retval + * Success - 0 + * @retval + * Error - <0 + */ +static int32_t DPC_ObjectDetection_deinit (DPM_DPCHandle handle) +{ + int32_t retVal = 0; + ObjDetObj *objDetObj = NULL; + uint8_t index; + + if (handle == NULL) + { + retVal = DPC_OBJDETRANGEHWA_EINVAL; + goto exit; + } + + objDetObj = handle; + + for(index = 0; index < RL_MAX_SUBFRAMES; index++) + { + if(objDetObj->subFrameObj[index].dpuRangeObj != NULL) + { + retVal = DPU_RangeProcHWA_deinit(objDetObj->subFrameObj[index].dpuRangeObj); + if (retVal != 0) + { + goto exit; + } + } + } + + /* Free DPC memory */ + MemoryP_ctrlFree(handle, sizeof(ObjDetObj)); + +exit: + return (retVal); +}