RTOS/IWR1642BOOST: TI-RTOS + SPI communication

/**
 *  senzorek MMW IWR 1642_onlinePrenosDat -> mereni range/Angle ... odesilame komplexni 32bit data Range/Angle
 *  verze 1.5.1
 *  zapocata dne 06.02.2018
 *
 * AZD Praha
 * autor: Michal Reznicek
 *
 *
 *  Jedna se o program kter� odes�l� data po UART do PC a lze je zobrazit v Matlabu
 *  Odeslan� data jsou " matice: complex(Range/Angle) " z jednoho odbehu zbytek odbehu
 *  pro dopplerovske zpracovani je ulozeny v radarCube poli.
 *  Odeslani dat je delano po UARTlogging ale dohoda zda data jsou nebo nejsou je provadena po
 *  Uart command kde se senzor ozve PC ze ma zmereno a muze sypat data do PC a PC mu potvrdi zda je
 *  taky pripraveny nebo neni. Vezkera komunikace je po konzoli.
 *
 *
 *  */



/**************************************************************************
 *************************** Include Files ********************************
 **************************************************************************/

/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <math.h>


/* BIOS/XDC Include Files. */
#include <xdc/std.h>
#include <xdc/cfg/global.h>
#include <xdc/runtime/IHeap.h>
#include <xdc/runtime/System.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/Memory.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Clock.h>
#include <ti/sysbios/heaps/HeapBuf.h>
#include <ti/sysbios/heaps/HeapMem.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/family/arm/v7a/Pmu.h>
#include <ti/sysbios/family/arm/v7r/vim/Hwi.h>

/* mmWave SDK Include Files: */
#include <ti/common/sys_common.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/esm/esm.h>
#include <ti/drivers/crc/crc.h>
#include <ti/drivers/uart/UART.h>
#include <ti/drivers/gpio/gpio.h>
#include <ti/drivers/mailbox/mailbox.h>
#include <ti/control/mmwave/mmwave.h>
#include <ti/utils/cli/cli.h>
#include <ti/utils/cli/include/cli_internal.h>
#include <ti/drivers/osal/DebugP.h>
#include <ti/drivers/osal/HwiP.h>
#include <ti/drivers/spi/SPI.h>
#include <ti/drivers/spi/include/mibspi.h>
#include <ti/drivers/dma/dma.h>
#include <ti/utils/testlogger/logger.h>

/* Demo Include Files */
#include "mss_mmw.h"
#include "ti/demo/xwr16xx/mmw/common/mmw_messages.h"


// pomocna promenna temp
volatile float tempf;

/*moje uzitecneukazatele na complexni signal*/
volatile int16_t *pDataIQ_I16;
volatile int32_t *pDataIQ_I32;
volatile uint16_t *pDataRangeAngle;

/**
 * @brief
 *  Global Variable for tracking information required by the mmw Demo
 */
MmwDemo_MCB    gMmwMssMCB;
radarFrameInfo gMmwMSSRadarFrameInfo;
DMA_Handle   dmaHandle;

/**************************************************************************
 *************************** Extern DefinitionsradarFrameInfo *******************************
 **************************************************************************/
/* CLI Init function */
void MmwDemo_CLIInit (void);
/*BSS init to default parameters */
int32_t MmwDemo_BSSinitToDefault (void);


/**************************************************************************
 ************************* Millimeter Wave Demo Functions Prototype**************
 **************************************************************************/
/*sensor stop*/
void MmwDemo_notifySensorStop(void);

/* Data path functions */
int32_t MmwDemo_mssDataPathConfig(void);
int32_t MmwDemo_mssDataPathStart(void);
int32_t MmwDemo_mssDataPathStop(void);

/* mmwave library call back fundtions */
void MmwDemo_mssMmwaveConfigCallbackFxn(MMWave_CtrlCfg* ptrCtrlCfg);
void MmwDemo_mssMmwaveStartCallbackFxn(void);
void MmwDemo_mssMmwaveStopcallbackFxn(void);
void MmwDemo_mssMmwaveEventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload);

/* MMW demo Task */
void MmwDemo_mssInitTask(UArg arg0, UArg arg1);
void MmwDemo_mmWaveCtrlTask(UArg arg0, UArg arg1);
void MmwDemo_mssCtrlPathTask(UArg arg0, UArg arg1);
void Mmw_sensorReadyToSendDataLoggingTask(UArg arg0, UArg arg1);
void Mmw_SPIDataLoggingTask(UArg arg0, UArg arg1);

/**************************************************************************
 ************************* Millimeter Wave Demo Functions **********************
 **************************************************************************/
/* CLI Command Functions */
extern int32_t MmwDemo_CLISensorStop (int32_t argc, char* argv[]);
extern int32_t MmwDemo_CLISensorStart (int32_t argc, char* argv[]);

extern MMWave_CtrlCfg  gCLIMMWaveControlCfg;
extern CLI_MCB gCLI;

int32_t MmwDemo_BSSinitToDefault (void)
{

    int32_t errCode;                        // promenna cislo chyby
    uint8_t                 index;
    rlProfileCfg_t          profileCfg;
    float                   tempMult_f;
    MMWave_ProfileHandle    profileHandle;
    rlChirpCfg_t            chirpCfg;

    char message[16] = {0};
    char *pmessage;
    // ukazatel na ukazatel zacatku zpravy
    pmessage = message;

    // zastaveni senzoru
    errCode = MmwDemo_CLISensorStop(0,&pmessage);

    // vykopneme pohrobky nastaveni, ktere mohou byt v pameti
    if (MMWave_flushCfg (gCLI.cfg.mmWaveHandle, &errCode) < 0)
    {
        /* Error: nastaveni nejde vyplivnout AJAJAJAJ co s tim hodime chybu no! */
        return errCode;
    }

    /* Reset the global configuration: */
    memset ((void*)&gCLIMMWaveControlCfg, 0, sizeof(MMWave_CtrlCfg));


    //data DFE mode - jestli jde o chirpy nebo o vysilani spojite
    gCLIMMWaveControlCfg.u.fullControlCfg.dfeDataOutputMode = MMWave_DFEDataOutputMode_CHIRP;
    // nastaveni kanalu analogovych tj povoleni Rx1 - Rx4 a d�le Tx1 a Tx2
    gCLIMMWaveControlCfg.u.fullControlCfg.chCfg.rxChannelEn = 15;
    gCLIMMWaveControlCfg.u.fullControlCfg.chCfg.txChannelEn = 3;
    gCLIMMWaveControlCfg.u.fullControlCfg.chCfg.cascading = 0;
    // nastavime ADC ... tj komplex signal + swapovani dat)
    gCLIMMWaveControlCfg.u.fullControlCfg.adcOutCfg.fmt.bitFormat.b2AdcBits = 2;
    gCLIMMWaveControlCfg.u.fullControlCfg.adcOutCfg.fmt.bitFormat.b2AdcOutFmt = 1;
    // nastaveni bufferu pro uchovavani vzorku po ADC
    gCLIMMWaveControlCfg.u.fullControlCfg.adcbufCfg.adcFmt = 0;
    gCLIMMWaveControlCfg.u.fullControlCfg.adcbufCfg.iqSwapSel = 0;
    gCLIMMWaveControlCfg.u.fullControlCfg.adcbufCfg.chInterleave = 1;
    gCLIMMWaveControlCfg.u.fullControlCfg.adcbufCfg.chirpThreshold = 1;

    // *****************  pripraveni konfigurace profilu !!!! tj start stop frekvence a filtry a podobne******************************
    memset ((void *)&profileCfg, 0, sizeof(rlProfileCfg_t));
    profileCfg.profileId             = 0;                                              // prvni parametr ... profile Identifier  musi byt 0
    tempMult_f = (float)(1U << 26);
    tempMult_f = (float)(77 * tempMult_f);
    tempMult_f = (tempMult_f) / ((float)(3.6));
    profileCfg.startFreqConst        = (uint32_t)(tempMult_f);           // start frekvence v GHz 77 GHz
    profileCfg.idleTimeConst         = (uint32_t)((float)(7) * 1000 / 10);             // idle time ... to je jak dlouho pada VCO pokud dorazi na konec rampy
                                                                                            // tj 81 GHz nez se dostane na 77 GHz.
    profileCfg.adcStartTimeConst     = (uint32_t)((float)(8) * 1000 / 10);             // ADC start time ... cas nez se zacne v zorkovat OKNO
                                                                                            // ktere sbira vzorky z chirpu .... cas je pocitan od
                                                                                            // okamziku kdy spadl VCO na 77 GHz.
    profileCfg.rampEndTime           = (uint32_t)((float)(42) * 1000 / 10);            // ramp end time us ... konec rampy
    profileCfg.txOutPowerBackoffCode =  (0);                                           // Tx output power back-off code for tx antennas  ... 0!
    profileCfg.txPhaseShifter        =  (0);                                           // tx phase shifter for tx antennas  ... vzdy 0!

    tempMult_f = (float)(1U << 26);
    tempMult_f = (float)(91 * tempMult_f);
    tempMult_f = (tempMult_f) / ((float)(3.6)*(float)(1e3*900));

    profileCfg.freqSlopeConst        = (uint32_t)(tempMult_f);  // frequency slope constant ... strmost chirpu
    profileCfg.txStartTime           = (uint32_t)( (float)(1) * 1000 / 10);            // tx start time in u-sec
    profileCfg.numAdcSamples         =  (96);                                          // number of ADC samples ... vyorkz po ADC
    profileCfg.digOutSampleRate      =  (3000);                                        // ADC sampling frequency in ksps ... vzorkovacka
    profileCfg.hpfCornerFreq1        =  (0);                                           // dvanacty parametr ... HPF1
                                                                                                // 0 ... 175 kHz
                                                                                                // 1 ... 235 kHz
                                                                                                // 2 ... 350 kHz
                                                                                                // 3 ... 700 kHz
    profileCfg.hpfCornerFreq2        =  (0);                                            // trinacty parametr ... HPF2
                                                                                                // 0 ... 350 kHz
                                                                                                // 1 ... 700 kHz
                                                                                                // 2 ... 1400 kHz
                                                                                                // 3 ... 2800 kHz
    profileCfg.rxGain                =  (52);                                           // rx gain in dB (valid values 24 to 48)

    // Search for a free space in the mmWave configuration block:
    for (index = 0U; index < MMWAVE_MAX_PROFILE; index++)
    {
        // Did we get a free entry?
        if (gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.profileHandle[index] == NULL)
        {
            // YES: We can add the profile.
            break;
        }
    }
    if (index == MMWAVE_MAX_PROFILE)
    {
        // Error: All the profiles have been exhausted
        return -1;
    }

    // Add the profile to the mmWave module:
    profileHandle = MMWave_addProfile (gCLI.cfg.mmWaveHandle, &profileCfg, &errCode);
    if (profileHandle == NULL)
    {
        // Error: Unable to add the profile. Return the error code back
        return errCode;
    }

    // Record the profile:
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.profileHandle[index] = profileHandle;


    // ******************************konfigurace chirp� 0 .*********************************
        memset ((void *)&chirpCfg, 0, sizeof(rlChirpCfg_t));

    /* Populate the chirp configuration: */
    chirpCfg.txEnable        = 1;

    /* Get the profile handle to which the chirp is to be added: */
    if (MMWave_getProfileHandle (gCLI.cfg.mmWaveHandle, chirpCfg.profileId,
                                 &profileHandle, &errCode) < 0)
    {
        /* Error: Unable to get the profile handle. Return the error code */
        return errCode;
    }

    /* Add the chirp to the profile */
    if (MMWave_addChirp (profileHandle, &chirpCfg, &errCode) == NULL)
    {
        /* Error: Unable to add the chirp. Return the error code. */
        return errCode;
    }
    // ******************************konfigurace chirp� 1.*********************************
        memset ((void *)&chirpCfg, 0, sizeof(rlChirpCfg_t));

    /* Populate the chirp configuration: */
    chirpCfg.chirpStartIdx   = 1;
    chirpCfg.chirpEndIdx     = 1;
    chirpCfg.txEnable        = 2;

    /* Get the profile handle to which the chirp is to be added: */
    if (MMWave_getProfileHandle (gCLI.cfg.mmWaveHandle, chirpCfg.profileId,
                                 &profileHandle, &errCode) < 0)
    {
        /* Error: Unable to get the profile handle. Return the error code */
        return errCode;
    }

    /* Add the chirp to the profile */
    if (MMWave_addChirp (profileHandle, &chirpCfg, &errCode) == NULL)
    {
        /* Error: Unable to add the chirp. Return the error code. */
        return errCode;
    }

    // ****** LOW power mode ******
    gCLIMMWaveControlCfg.u.fullControlCfg.lowPowerMode.anaChannelCfg = 0;
    gCLIMMWaveControlCfg.u.fullControlCfg.lowPowerMode.lpAdcMode = 0;
    // *********************nastaveni ramce ****************************
    /* Save Configuration to use later */
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.chirpStartIdx = 0;
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.chirpEndIdx = 1;
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.numLoops = 16;
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.numFrames = 1;
    tempMult_f = 300*1000000;
    tempMult_f = tempMult_f/5;
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.framePeriodicity =  (uint32_t)(tempMult_f);
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.triggerSelect      = 1;
    gCLIMMWaveControlCfg.u.fullControlCfg.u.chirpModeCfg.frameCfg.frameTriggerDelay  = 0;


    MmwDemo_notifySensorStart(true);
    MmwDemo_mssDataPathStart();

    return 0;

}


/**
 *  @b Description
 *  @n
 *      Registered event function 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
 *      Not applicable
 */
void MmwDemo_mssMmwaveEventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload)
{
    uint16_t asyncSB = RL_GET_SBID_FROM_UNIQ_SBID(sbId);

#if 0
    System_printf ("Debug: BSS Event MsgId: %d [Sub Block Id: %d Sub Block Length: %d]\n",
                    msgId, sbId, sbLen);
#endif

    /* Process the received message: */
    switch (msgId)
    {
        case RL_RF_ASYNC_EVENT_MSG:
        {
            /* Received Asychronous Message: */
            switch (asyncSB)
            {
                case RL_RF_AE_CPUFAULT_SB:
                {
                    /* Post event to datapath task notify BSS events */
                    Event_post(gMmwMssMCB.eventHandle, MMWDEMO_BSS_CPUFAULT_EVT);
                    break;
                }
                case RL_RF_AE_ESMFAULT_SB:
                {
                    /* Post event to datapath task notify BSS events */
                    Event_post(gMmwMssMCB.eventHandle, MMWDEMO_BSS_ESMFAULT_EVT);
                    break;
                }
                case RL_RF_AE_INITCALIBSTATUS_SB:
                {
                    /* This event should be handled by mmwave internally, ignore the event here */
                    break;
                }

                case RL_RF_AE_FRAME_TRIGGER_RDY_SB:
                {
                    /* This event is not handled on MSS */
                    break;
                }
                case RL_RF_AE_MON_TIMING_FAIL_REPORT_SB:
                {
                    /* Increment the statistics for the number of failed reports */
                    gMmwMssMCB.stats.numFailedTimingReports++;

                    #if 0 
                    /* if something needs to be done then need to implement the function
                       to handle the event below in MmwDemo_mssCtrlPathTask()*/
                            
                    /* Post event to datapath task notify BSS events */
                    Event_post(gMmwMssMCB.eventHandle, MMWDEMO_BSS_MONITORING_REP_EVT);
                    #endif
                    break;
                }
                case RL_RF_AE_RUN_TIME_CALIB_REPORT_SB:
                {
                    /* Increment the statistics for the number of received calibration reports */
                    gMmwMssMCB.stats.numCalibrationReports++;

                    #if 0 
                    /* if something needs to be done then need to implement the function
                       to handle the event below in MmwDemo_mssCtrlPathTask()*/
                            
                    /* Post event to datapath task notify BSS events */
                    Event_post(gMmwMssMCB.eventHandle, MMWDEMO_BSS_CALIBRATION_REP_EVT);
                    #endif
                    break;
                }
                default:
                {
                    System_printf ("Error: Asynchronous Event SB Id %d not handled\n", asyncSB);
                    break;
                }
            }
            break;
        }
        default:
        {
            System_printf ("Error: Asynchronous message %d is NOT handled\n", msgId);
            break;
        }
    }

    return;
}

/**
 *  @b Description
 *  @n
 *      Application registered callback function which is invoked after the configuration
 *      has been used to configure the mmWave link and the BSS. This is applicable only for
 *      the XWR16xx. The BSS can be configured only by the MSS *or* DSS. The callback API is
 *      triggered on the remote execution domain (which did not configure the BSS)
 *
 *  @param[in]  ptrCtrlCfg
 *      Pointer to the control configuration
 *
 *  @retval
 *      Not applicable
 */
void MmwDemo_mssMmwaveConfigCallbackFxn(MMWave_CtrlCfg* ptrCtrlCfg)
{
    /* For mmw Demo, mmwave_config() will always be called from MSS, 
       due to the fact CLI is running on MSS, hence this callback won't be called */

    gMmwMssMCB.stats.datapathConfigEvt ++;
}


void Mmw_SPIDataLoggingTask(UArg arg0, UArg arg1)
{
    SPI_Params      spiParams;
    SPI_Transaction spiTransaction;
    int32_t ret;

    System_printf("Start SPI task\n");
    /*****************************************************************************
     * SPI nastaveni
     *****************************************************************************/
    SPI_Params_init(&spiParams);
    spiParams.mode = SPI_MASTER;
    spiParams.transferCallbackFxn = NULL;
    spiParams.transferMode = SPI_MODE_BLOCKING;
    spiParams.transferTimeout = SPI_WAIT_FOREVER;
    spiParams.u.masterParams.bitRate = 1000000;
    spiParams.u.masterParams.numSlaves = 1;
    spiParams.u.masterParams.slaveProf[0].chipSelect = 0;
    spiParams.u.masterParams.slaveProf[0].ramBufLen = MIBSPI_RAM_MAX_ELEM;
    //spiParams.u.masterParams.slaveProf[0].dmaCfg.txDmaChanNum = 1U;
    //spiParams.u.masterParams.slaveProf[0].dmaCfg.rxDmaChanNum = 0U;
    spiParams.frameFormat = SPI_POL0_PHA0;
    spiParams.shiftFormat = SPI_MSB_FIRST;

    spiParams.dmaEnable = 0;
    //spiParams.dmaHandle = dmaHandle;
    spiParams.shiftFormat = SPI_MSB_FIRST;
    spiParams.pinMode   =  SPI_PINMODE_3PIN; // SPI_PINMODE_4PIN_CS;//SPI_PINMODE_3PIN;
    spiParams.eccEnable = 1;

    gMmwMssMCB.handleSPI = SPI_open(0u, &spiParams);
    if(!gMmwMssMCB.handleSPI)
    {
        System_printf("SPI didn't open \n");
    }
    else
    {
        System_printf("SPI opened successfully \n");
    }
    /*****************************************************************************
     * Smi�ka Tasku
     *****************************************************************************/
    while(1)
    {
        char txBffer;
        char rxBuffer;

        txBffer = 0x48;
        rxBuffer = 0x0;

        spiTransaction.count = 1;
        spiTransaction.txBuf = &txBffer;
        spiTransaction.rxBuf = &rxBuffer;
        spiTransaction.slaveIndex = 0;

        ret = SPI_transfer(gMmwMssMCB.handleSPI, &spiTransaction);
        if (!ret)
        {
            System_printf("Unsuccessful SPI transfer");
        }
        else
        {
            System_printf(" SPI transfer OK ");
        }
    }
}

/*
 *  @b Description
 *  @n
 *
 *
 * */

void Mmw_sensorReadyToSendDataLoggingTask(UArg arg0, UArg arg1)
{
    while(1)
    {
        /* Cekani na vyvolani udalosti ze data byla prijata a zpracovana do radarCubeMatice
         * a zaroven se nam hlasi PC ze chce prijmout data po UART logging kanalu od seriove linky
         *  */
        Event_pend(gMmwMssMCB.eventUARTlogging,
                        MMW_SENSOR_HAS_NEW_DATA | MMW_PC_IS_READY_FOR_DATA,
                        (Event_Id_NONE),
                        BIOS_WAIT_FOREVER);

        /************************************************************************
         * ODESLANI DAT
         ****************************************&********************************/
        // ulozeni odkazu do L3 na RADAR CUBE data

        switch(gMmwMSSRadarFrameInfo.howDataSend)
        {
            case MMW_SEND_RANGE_DOPPLER:
            {
                pDataIQ_I16 = (int16_t *)(SOC_XWR16XX_MSS_L3RAM_BASE_ADDRESS);
                UART_writePolling (gMmwMssMCB.loggingUartHandle,(uint8_t *)pDataIQ_I16, gMmwMSSRadarFrameInfo.celkemPocetDatRangeDoppler );
                /*
                pDataIQ_I32 = (int32_t *)(SOC_XWR16XX_MSS_L3RAM_BASE_ADDRESS + gMmwMSSRadarFrameInfo.celkemPocetDatRangeDoppler + gMmwMSSRadarFrameInfo.celkemBrangeAngle);
                UART_writePolling (gMmwMssMCB.loggingUartHandle,(uint8_t *)pDataIQ_I32, gMmwMSSRadarFrameInfo.celkemBrangeAngle );
                */
                //System_printf ("MSS: data odeslana po UART logging \n");
                break;
            }
            case MMW_SEND_RANGE_ANGLE:
            {

                pDataRangeAngle = (uint16_t *)(SOC_XWR16XX_MSS_L3RAM_BASE_ADDRESS + gMmwMSSRadarFrameInfo.celkemPocetDatRangeDoppler);
                UART_writePolling (gMmwMssMCB.loggingUartHandle,(uint8_t *)pDataRangeAngle, gMmwMSSRadarFrameInfo.celkemBrangeAngle );
                //UART_writePolling (gMmwMssMCB.loggingUartHandle,(uint8_t *)pDataRangeAngle, 4096u);

                System_printf ("MSS: data odeslana po UART logging \n");
                break;
            }
            default:
            {
                System_printf ("Neco se posralo nevim jaky data odeslat \n");
                break;
            }
        }
        MmwDemo_notifySensorStart(false);
        MmwDemo_mssDataPathStart();
        //MmwDemo_mssDataPathConfig();

    }
}

/**
 *  @b Description
 *  @n
 *      Application registered callback function which is invoked the mmWave link on BSS
 *      has been started. This is applicable only for the XWR16xx. The BSS can be configured
 *      only by the MSS *or* DSS. The callback API is triggered on the remote execution
 *      domain (which did not configure the BSS)
 *
 *  @retval
 *      Not applicable
 */
void MmwDemo_mssMmwaveStartCallbackFxn(void)
{
    /* Post an event to main data path task. 
       This function in only called when mmwave_start() is called on DSS */
    gMmwMssMCB.stats.datapathStartEvt ++;

    /* Post event to start is done */
    Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_START_COMPLETED_EVT);
}

void MmwDemo_mssMmwaveSPICallbackFxn(void)
{
    Event_post(gMmwMssMCB.eventSPIhandle, MMW_PC_SPI_DONE);
}

/**
 *  @b Description
 *  @n
 *      Application registered callback function which is invoked the mmWave link on BSS
 *      has been ped. This is applicable only for the XWR16xx. The BSS can be configured
 *      only by the MSS *or* DSS. The callback API is triggered on the remote execution
 *      domain (which did not configure the BSS)
 *
 *  @retval
 *      Not applicable
 */
void MmwDemo_mssMmwaveStopCallbackFxn(void)
{
    /* Possible sceanarios:
       1. CLI sensorStop command triggers mmwave_stop() to be called from MSS 
       2. In case of Error, mmwave_stop() will be triggered either from MSS or DSS
     */
    gMmwMssMCB.stats.datapathStopEvt ++;
}

/**
 *  @b Description
 *  @n
 *      Function to send a message to peer through Mailbox virtural channel 
 *
 *  @param[in]  message
 *      Pointer to the MMW demo message.  
 *
 *  @retval
 *      Success    - 0
 *      Fail       < -1 
 */
int32_t MmwDemo_mboxWrite(MmwDemo_message     * message)
{
    int32_t                  retVal = -1;
    
    retVal = Mailbox_write (gMmwMssMCB.peerMailbox, (uint8_t*)message, sizeof(MmwDemo_message));
    if (retVal == sizeof(MmwDemo_message))
    {
        retVal = 0;
    }
    return retVal;
}


/**
 *  @b Description
 *  @n
 *      The Task is used to handle  the mmw demo messages received from 
 *      Mailbox virtual channel.  
 *
 *  @param[in]  arg0
 *      arg0 of the Task. Not used
 *  @param[in]  arg1
 *      arg1 of the Task. Not used
 *
 *  @retval
 *      Not Applicable.
 */
static void MmwDemo_mboxReadTask(UArg arg0, UArg arg1)
{
    MmwDemo_message      message;
    int32_t              retVal = 0;


    /* wait for new message and process all the messsages received from the peer */

    while(1)
    {
        Semaphore_pend(gMmwMssMCB.mboxSemHandle, BIOS_WAIT_FOREVER);
        


        /* Read the message from the peer mailbox: We are not trying to protect the read
         * from the peer mailbox because this is only being invoked from a single thread */
        retVal = Mailbox_read(gMmwMssMCB.peerMailbox, (uint8_t*)&message, sizeof(MmwDemo_message));
        if (retVal < 0)
        {
            /* Error: Unable to read the message. Setup the error code and return values */
            System_printf ("Error: Mailbox read failed [Error code %d]\n", retVal);
        }
        else if (retVal == 0)
        {
            /* We are done: There are no messages available from the peer execution domain. */
            continue;
        }
        else
        {
            /* Flush out the contents of the mailbox to indicate that we are done with the message. This will
             * allow us to receive another message in the mailbox while we process the received message. */
            Mailbox_readFlush (gMmwMssMCB.peerMailbox);

            /* Process the received message: */
            switch (message.type)
            {

                case MMWDEMO_DSS2MSS_STOPDONE:

                    Event_post(gMmwMssMCB.eventUARTlogging, MMW_SENSOR_HAS_NEW_DATA);

                    break;

                case MMW_DSS2MSS_PRENOS_1DFFT_READY:

                    //MmwDemo_notifySensorStop();
                    MmwDemo_mssDataPathStop();

                    /* Start the mmWave module: The configuration has been applied successfully. */
                    // MMWave_stop (gMmwMssMCB.ctrlHandle, &errCode);
                    /* Post sensorSTOP event to notify sensor stop command */
                    /*co se t��e adresace pam�t� tak
                     *
                     * pro HS RAM:
                     *          - SOC_XWR16XX_DSS_HSRAM_BASE_ADDRESS          0x21080000U
                     *          - SOC_XWR16XX_MSS_HSRAM_BASE_ADDRESS          0x52080000U
                     *          delka dat:
                     *          - SOC_XWR16XX_DSS_HSRAM_SIZE                  0x8000U
                     *          - SOC_XWR16XX_MSS_HSRAM_SIZE                  0x8000U
                     * pro L3 RAM:
                     *          - SOC_XWR16XX_DSS_L3RAM_BASE_ADDRESS          0x20000000U
                     *          - SOC_XWR16XX_MSS_L3RAM_BASE_ADDRESS          0x51000000U
                     *          delka dat:
                     *          - SOC_XWR16XX_DSS_L3RAM_SIZE                  0xA0000U
                     *          - SOC_XWR16XX_MSS_L3RAM_SIZE                  0xA0000U
                     * Ulozena data po 1Dfft
                     */

                    gMmwMSSRadarFrameInfo.celkemPocetDatRangeDoppler = message.getInfo.numDopplerCells * message.getInfo.numRangeCells
                            *  message.getInfo.numVirtualAntennas * sizeof(cmplx16ImRe_t);

                    break;

                case MMW_DSS2MSS_PRENOS_2DFFT_READY:

                    MmwDemo_mssDataPathStop();
                    // pokud jsou pripraveny data ANGLE/DOPPLER musio byt i 1D fft spoctena
                    gMmwMSSRadarFrameInfo.celkemPocetDatRangeDoppler = message.getInfo.numDopplerCells * message.getInfo.numRangeCells
                            * message.getInfo.numVirtualAntennas * sizeof(cmplx16ImRe_t);
                    gMmwMSSRadarFrameInfo.celkemBrangeAngle = ( message.getInfo.numRangeCells) * message.getInfo.numVirtualAntennas
                            * sizeof(uint32_t) * message.getInfo.numDopplerCells;
                    //System_printf ("Debug: frame hotovo \n");

                    break;

                default:
                {
                    /* Message not support */
                    System_printf ("Error: unsupport Mailbox message id=%d\n", message.type);
                    break;
                }
            }
        }
    }
}

/**
 *  @b Description
 *  @n
 *      This function is a callback funciton that invoked when a message is received from the peer.
 *
 *  @param[in]  handle
 *      Handle to the Mailbox on which data was received
 *  @param[in]  peer
 *      Peer from which data was received
 
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_mboxCallback
(
    Mailbox_Handle  handle,
    Mailbox_Type    peer
)
{
    /* Message has been received from the peer endpoint. Wakeup the mmWave thread to process
     * the received message. */
    Semaphore_post (gMmwMssMCB.mboxSemHandle);
}

/**
 *  @b Description
 *  @n
 *      Function to do Data Path Configuration on MSS. After received Configuration from
 *    CLI, this function will start the system configuration process, inclucing mmwaveLink, BSS
 *    and DSS.
 *
 *  @retval
 *      0  - Success.
 *      <0 - Failed with errors
 */
int32_t MmwDemo_mssDataPathConfig(void)
{
    int32_t  errCode;
    
    /* Setup the calibration frequency: */    
    gMmwMssMCB.cfg.ctrlCfg.freqLimitLow  = 760U;
    gMmwMssMCB.cfg.ctrlCfg.freqLimitHigh = 810U;
    
    /* Configure the mmWave module: */
    /*konfigurace modulu MMW prob�ha pouze z MSS protoze MSS prij�ma �id�c� informace po UARTU */

    if (MMWave_config (gMmwMssMCB.ctrlHandle, &gMmwMssMCB.cfg.ctrlCfg, &errCode) < 0)
    {
        System_printf ("Error: MMWDemoMSS mmWave Configuration failed [Error code %d]\n", errCode);
        return -1;
    }

    return 0;
}

/**
 *  @b Description
 *  @n
 *      Function to do Data Path Start on MSS. After received SensorStart command, MSS will 
 *    start all data path componets including mmwaveLink, BSS and DSS.
 *
 *  @retval
 *      0  - Success.
 *      <0 - Failed with errors
 */
int32_t MmwDemo_mssDataPathStart(void)
{
    int32_t    errCode;
    MMWave_CalibrationCfg   calibrationCfg;

    /* Initialize the calibration configuration: */
    memset ((void*)&calibrationCfg, 0, sizeof(MMWave_CalibrationCfg));

    /* Populate the calibration configuration: */
    calibrationCfg.enableCalibration    = true;
    calibrationCfg.enablePeriodicity    = true;
    calibrationCfg.periodicTimeInFrames = 10U;

    /* Start the mmWave module: The configuration has been applied successfully. */
    if (MMWave_start (gMmwMssMCB.ctrlHandle, &calibrationCfg, &errCode) < 0)
    {
        /* Error: Unable to start the mmWave control */
        System_printf ("Error: MMWDemoMSS mmWave Start failed [Error code %d]\n", errCode);
        return -1;
    }
    System_printf ("Debug: MMWDemoMSS mmWave Start succeeded \n");
    return 0;
}

/**
 *  @b Description
 *  @n
 *      Function to do Data Path Start on MSS. After received SensorStart command, MSS will 
 *    start all data path componets including mmwaveLink, BSS and DSS.
 *
 *  @retval
 *      0  - Success.
 *      <0 - Failed with errors
 */
int32_t MmwDemo_mssDataPathStop(void)
{
    int32_t    errCode;
    
    /* Stop the mmWave module: The configuration has been applied successfully. */
    if (MMWave_stop (gMmwMssMCB.ctrlHandle, &errCode) < 0)
    {
        /* Error: Unable to start the mmWave control */
        System_printf ("Error: MMWDemoMSS mmWave Stop failed [Error code %d]\n", errCode);
        return -1;
    }

    return 0;
}

/**
 *  @b Description
 *  @n
 *      The task is used to provide an execution context for the mmWave
 *      control task
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_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);
    }
}

/**
 *  @b Description
 *  @n
 *      This is a utility function which can be invoked from the CLI or
 *      the Switch press to start the sensor. This sends an event to the
 *      sensor management task where the actual *start* procedure is
 *      implemented.
 *
 *  @retval
 *      Not applicable
 */
void MmwDemo_notifySensorStart(bool doReconfig)
{
    gMmwMssMCB.stats.cliSensorStartEvt ++;

   if (doReconfig) {
        /* Get the configuration from the CLI mmWave Extension */
        CLI_getMMWaveExtensionConfig (&gMmwMssMCB.cfg.ctrlCfg);
            
            /* Post sensorStart event to start reconfig and then start the sensor */
        Event_post(gMmwMssMCB.eventHandle, MMWDEMO_CLI_SENSORSTART_EVT);
    }
    else
    {
        /* Post frameStart event to skip reconfig and just start the sensor */
        Event_post(gMmwMssMCB.eventHandle, MMWDEMO_CLI_FRAMESTART_EVT);
    }
}

/**
 *  @b Description
 *  @n
 *      This is a utility function which can be invoked from the CLI or
 *      the Switch press to start the sensor. This sends an event to the
 *      sensor management task where the actual *start* procedure is
 *      implemented.
 *
 *  @retval
 *      Not applicable
 */
void MmwDemo_notifySensorStop(void)
{
    gMmwMssMCB.stats.cliSensorStopEvt ++;

    /* Post sensorSTOP event to notify sensor stop command */
    Event_post(gMmwMssMCB.eventHandle, MMWDEMO_CLI_SENSORSTOP_EVT);
}

/**
 *  @b Description
 *  @n
 *      This is a utility function which can be used by the CLI to 
 *      pend for start complete (after MmwDemo_notifySensorStart is called)
 *
 *  @retval
 *      Not applicable
 */
int32_t MmwDemo_waitSensorStartComplete(void)
{
    UInt          event;
    int32_t       retVal;
    /* Pend on the START NOTIFY event */
    event = Event_pend(gMmwMssMCB.eventHandleNotify, 
                          Event_Id_NONE, 
                          MMWDEMO_DSS_START_COMPLETED_EVT | MMWDEMO_DSS_START_FAILED_EVT,
                          BIOS_WAIT_FOREVER);

    /************************************************************************
     * DSS event:: START notification
     ************************************************************************/
    if(event & MMWDEMO_DSS_START_COMPLETED_EVT)
    {
        /* Sensor has been started successfully */
        gMmwMssMCB.isSensorStarted = true;
        /* Turn on the LED */
        GPIO_write (SOC_XWR16XX_GPIO_2, 1U);
        retVal = 0;
    }
    else if(event & MMWDEMO_DSS_START_FAILED_EVT)
    {
        /* Sensor start failed */
        gMmwMssMCB.isSensorStarted = false;
        retVal = -1;
    } 
    else 
    {
        /* we should block forever till we get the events. If the desired event 
           didnt happen, then throw an assert */
        retVal = -1;
        DebugP_assert(0);
    }
    return retVal;
}

/**
 *  @b Description
 *  @n
 *      This is a utility function which can be used by the CLI to 
 *      pend for stop complete (after MmwDemo_notifySensorStop is called)
 *
 *  @retval
 *      Not applicable
 */
void MmwDemo_waitSensorStopComplete(void)
{
    UInt          event;
    /* Pend on the START NOTIFY event */
    event = Event_pend(gMmwMssMCB.eventHandleNotify, 
                          Event_Id_NONE, 
                          MMWDEMO_DSS_STOP_COMPLETED_EVT,
                          BIOS_WAIT_FOREVER);

    /************************************************************************
     * DSS event:: STOP notification
     ************************************************************************/
    if(event & MMWDEMO_DSS_STOP_COMPLETED_EVT)
    {
        /* Sensor has been stopped successfully */
        gMmwMssMCB.isSensorStarted = false;
        
        /* Turn off the LED */
        GPIO_write (SOC_XWR16XX_GPIO_2, 0U);
        
        /* print for user */
        System_printf("Sensor has been stopped\n");
    }
    else {
        /* we should block forever till we get the event. If the desired event 
           didnt happen, then throw an assert */
        DebugP_assert(0);
    }
}



/**
 *  @b Description
 *  @n
 *      Callback function invoked when the GPIO switch is pressed.
 *      This is invoked from interrupt context.
 *
 *  @param[in]  index
 *      GPIO index configured as input
 *
 *  @retval
 *      Not applicable
 */
static void MmwDemo_switchPressFxn(unsigned int index)
{
    /* Was the sensor started? */
    if (gMmwMssMCB.isSensorStarted == true)
    {
        /* YES: We need to stop the sensor now */
        MmwDemo_notifySensorStop();
    }
    else
    {
        /* NO: We need to start the sensor now. */
        MmwDemo_notifySensorStart(true);
    }
}

/**
 *  @b Description
 *  @n
 *      The task is used to process data path events
 *      control task
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_mssCtrlPathTask(UArg arg0, UArg arg1)
{
    UInt          event;

    /**********************************************************************
     * Setup the PINMUX:
     * - GPIO Input : Configure pin J13 as GPIO_1 input
     * - GPIO Output: Configure pin K13 as GPIO_2 output
     **********************************************************************/
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINJ13_PADAC, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINJ13_PADAC, SOC_XWR16XX_PINJ13_PADAC_GPIO_1);
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINK13_PADAZ, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINK13_PADAZ, SOC_XWR16XX_PINK13_PADAZ_GPIO_2);

    /**********************************************************************
     * Setup the SW1 switch on the EVM connected to GPIO_1
     * - This is used as an input
     * - Enable interrupt to be notified on a switch press
     **********************************************************************/
    GPIO_setConfig (SOC_XWR16XX_GPIO_1, GPIO_CFG_INPUT | GPIO_CFG_IN_INT_RISING | GPIO_CFG_IN_INT_LOW);
    GPIO_setCallback (SOC_XWR16XX_GPIO_1, MmwDemo_switchPressFxn);
    GPIO_enableInt (SOC_XWR16XX_GPIO_1);

    /**********************************************************************
     * Setup the DS3 LED on the EVM connected to GPIO_2
     **********************************************************************/
    GPIO_setConfig (SOC_XWR16XX_GPIO_2, GPIO_CFG_OUTPUT);

    /* Data Path management task Main loop */
    while (1)
    {
        event = Event_pend(gMmwMssMCB.eventHandle, 
                          Event_Id_NONE, 
                          MMWDEMO_CLI_EVENTS | MMWDEMO_BSS_FAULT_EVENTS,
                          BIOS_WAIT_FOREVER); 

        /************************************************************************
         * CLI event:: SensorStart
         ************************************************************************/

        if(event & MMWDEMO_CLI_SENSORSTART_EVT)
        {
            System_printf ("Debug: MMWDemoMSS Received CLI sensorStart Event\n");
        
            /* Setup the data path: */
            if(gMmwMssMCB.isSensorStarted==false) // test zda neni mmw spusteny
            {
                // odesleme nastaveni senzoru
                if (MmwDemo_mssDataPathConfig () < 0) {
                    /* Post start failed event; error printing is done in function above */
                    Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_START_FAILED_EVT);
                    continue;
                }
                else
                {
                    /* start complete event is posted via DSS */
                }
            }
            else
            {
                /* Post start complete event as this is just a duplicate command */
                Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_START_COMPLETED_EVT);
                continue;
            }
        }

        /************************************************************************
         * CLI event:: SensorStop
         ************************************************************************/
        if(event & MMWDEMO_CLI_SENSORSTOP_EVT)
        {
            if(gMmwMssMCB.isSensorStarted==true)
            {
                if (MmwDemo_mssDataPathStop () < 0)
                {
                    /* do we need stop fail event; for now just mark the stop as completed */
                    Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_STOP_COMPLETED_EVT);
                    continue;
                }
                else
                {
                    /* DSS will post the stop completed event */
                }
            }
            else
            {
                /* Post stop complete event as this is just a duplicate command */
                Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_STOP_COMPLETED_EVT);
                continue;
            }
        }

        /************************************************************************
         * CLI event:: Framestart
         ************************************************************************/
        if(event & MMWDEMO_CLI_FRAMESTART_EVT)
        {
            /* error printing happens inside this function */
            if(gMmwMssMCB.isSensorStarted==false) 
            {
                if (MmwDemo_mssDataPathStart () < 0) {
                    /* Post start failed event; error printing is done in function above */
                    Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_START_FAILED_EVT);
                    continue;
                }
            }

            /* Post event to start is done */
            Event_post(gMmwMssMCB.eventHandleNotify, MMWDEMO_DSS_START_COMPLETED_EVT);
        }

        /************************************************************************
         * BSS event:: CPU fault
         ************************************************************************/
        if(event & MMWDEMO_BSS_CPUFAULT_EVT)
        {
            DebugP_assert(0);
            break;
        }

        /************************************************************************
         * BSS event:: ESM fault
         ************************************************************************/
        if(event & MMWDEMO_BSS_ESMFAULT_EVT)
        {
            DebugP_assert(0);
            break;
        }

        
        

    }

    System_printf("Debug: MMWDemoDSS Data path exit\n");
}


/**
 *  @b Description
 *  @n
 *      System Initialization Task which initializes the various
 *      components in the system.
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_mssInitTask(UArg arg0, UArg arg1)
{
    int32_t             errCode;
    MMWave_InitCfg      initCfg;
    UART_Params         uartParams;
    Task_Params         taskParams;
    Semaphore_Params    semParams;
    Mailbox_Config      mboxCfg;
    Error_Block         eb;

    /* Debug Message: */
    System_printf("Debug: MMWDemoMSS Launched the Initialization Task\n");

    /*****************************************************************************
     * Initialize the mmWave SDK components:
     *****************************************************************************/
    /* Pinmux setting */

    /* Setup the PINMUX to bring out the UART-1 */
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINN5_PADBE, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);    
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINN5_PADBE, SOC_XWR16XX_PINN5_PADBE_MSS_UARTA_TX);
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINN4_PADBD, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);    
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINN4_PADBD, SOC_XWR16XX_PINN4_PADBD_MSS_UARTA_RX);

    /* Setup the PINMUX to bring out the UART-3 */
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINF14_PADAJ, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINF14_PADAJ, SOC_XWR16XX_PINF14_PADAJ_MSS_UARTB_TX);

    /* Setup the PINMUX to bring out the DSS UART */
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINP8_PADBM, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINP8_PADBM, SOC_XWR16XX_PINP8_PADBM_DSS_UART_TX);

    // .......................nastaveni PINU SPI periferii..............................
    //  SPIA .... pinout na kitu IWR1642 boost:
    //  signal......nozicka na chipu...cislo konektoru...nozicka na konektoru
    //  AR_MISO1        E14                  J5                 14
    //  AR_MOSI         D13                  J5                 12
    //  AR_CS1          C13                  J5                 6
    //  AR_SPICLK1      E13                  J6                 13
    /* Setup the PINMUX to bring out the UART-1 */

    //MISO
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINE14_PADAE, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINE14_PADAE, SOC_XWR16XX_PINE14_PADAE_SPIA_MISO );
    //Pinmux_Set_Pull(SOC_XWR16XX_PINE14_PADAE,PINMUX_PULLUP_EN);
    //MOSI
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PIND13_PADAD, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PIND13_PADAD, SOC_XWR16XX_PIND13_PADAD_SPIA_MOSI );
    //Pinmux_Set_Pull(SOC_XWR16XX_PIND13_PADAD,PINMUX_PULLUP_EN);
    //CS
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINC13_PADAG, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINC13_PADAG, SOC_XWR16XX_PINC13_PADAG_SPIA_CSN );
    Pinmux_Set_Pull(SOC_XWR16XX_PINC13_PADAG,PINMUX_PULLUP_EN);
    //CLK
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINE13_PADAF, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINE13_PADAF, SOC_XWR16XX_PINE13_PADAF_SPIA_CLK );
    // SPI_HOST_INTR - not used, reference code
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINP13_PADAA, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINP13_PADAA, SOC_XWR16XX_PINP13_PADAA_SPI_HOST_INTR);




    /* Initialize the UART */
    UART_init();

    /* Initialize the GPIO */
    GPIO_init();

    /* Initialize the Mailbox */
    Mailbox_init(MAILBOX_TYPE_MSS);
    /*    Inicializace SPI */

    SPI_init();
    if (SOC_SPIOutputCtrl(gMmwMssMCB.socHandle, 0, 1, &errCode) < 0)
    {
      /* Debug Message: */
        System_printf("Debug: SOC_SPIOutputCtrl failed with Error [%d]\n", errCode);
        return;
    }

    /*****************************************************************************
     * Open & configure the drivers:
     *****************************************************************************/

    /* Setup the default UART Parameters */
    UART_Params_init(&uartParams);
    uartParams.clockFrequency  = gMmwMssMCB.cfg.sysClockFrequency;
    uartParams.baudRate        = gMmwMssMCB.cfg.commandBaudRate;
    uartParams.isPinMuxDone    = 1U;
    uartParams.readEcho        = UART_ECHO_OFF;

    /* Open the UART Instance */
    gMmwMssMCB.commandUartHandle = UART_open(0, &uartParams);
    if (gMmwMssMCB.commandUartHandle == NULL)
    {
        System_printf("Error: MMWDemoMSS Unable to open the Command UART Instance\n");
        return;
    }

    /* Setup the default UART Parameters */
    UART_Params_init(&uartParams);
    uartParams.writeDataMode  = UART_DATA_BINARY;
    uartParams.readDataMode   = UART_DATA_BINARY;
    uartParams.clockFrequency = gMmwMssMCB.cfg.sysClockFrequency;
    uartParams.baudRate       = gMmwMssMCB.cfg.loggingBaudRate;
    uartParams.isPinMuxDone   = 1U;
    uartParams.readEcho       = UART_ECHO_OFF;

    /* Open the Logging UART Instance: */
    gMmwMssMCB.loggingUartHandle = UART_open(1u, &uartParams);
    if (gMmwMssMCB.loggingUartHandle == NULL)
    {
        System_printf("Error: MMWDemoMSS Unable to open the Logging UART Instance\n");
        return;
    }

    /*****************************************************************************
     * Creating communication channel between MSS & DSS
     *****************************************************************************/

    /* Create a binary semaphore which is used to handle mailbox interrupt. */
    Semaphore_Params_init(&semParams);
    semParams.mode           = Semaphore_Mode_BINARY;
    gMmwMssMCB.mboxSemHandle = Semaphore_create(0, &semParams, NULL);

    /* Setup the default mailbox configuration */
    Mailbox_Config_init(&mboxCfg);

    /* Setup the configuration: */
    mboxCfg.chType       = MAILBOX_CHTYPE_MULTI;
    mboxCfg.chId         = MAILBOX_CH_ID_0;
    mboxCfg.writeMode    = MAILBOX_MODE_BLOCKING;
    mboxCfg.readMode     = MAILBOX_MODE_CALLBACK;
    mboxCfg.readCallback = &MmwDemo_mboxCallback;
    uartParams.readEcho  = UART_ECHO_OFF;

    /* Initialization of Mailbox Virtual Channel  */
    gMmwMssMCB.peerMailbox = Mailbox_open(MAILBOX_TYPE_DSS, &mboxCfg, &errCode);
    if (gMmwMssMCB.peerMailbox == NULL)
    {
        /* Error: Unable to open the mailbox */
        System_printf("Error: Unable to open the Mailbox to the DSS [Error code %d]\n", errCode);
        return;
    }

    /* Create task to handle mailbox messges */
    Task_Params_init(&taskParams);
    taskParams.stackSize = 16*1024;
    Task_create(MmwDemo_mboxReadTask, &taskParams, NULL);


    /*****************************************************************************
     * Create Event to handle mmwave callback and system datapath events 
     *****************************************************************************/
    /* Default instance configuration params */
    Error_init(&eb);
    gMmwMssMCB.eventUARTlogging = Event_create(NULL, &eb);
    if (gMmwMssMCB.eventUARTlogging == NULL)
    {
        DebugP_assert(0);
        return ;
    }
    
    /* Default instance configuration params */
    Error_init(&eb);
    gMmwMssMCB.eventHandle = Event_create(NULL, &eb);
    if (gMmwMssMCB.eventHandle == NULL)
    {
        DebugP_assert(0);
        return ;
    }

    Error_init(&eb);
    gMmwMssMCB.eventHandleNotify = Event_create(NULL, &eb);
    if (gMmwMssMCB.eventHandleNotify == NULL) 
    {
        DebugP_assert(0);
        return ;
    }

    /*****************************************************************************
     * mmWave: Initialization of the high level module
     *****************************************************************************/

    /* Initialize the mmWave control init configuration */
    memset ((void*)&initCfg, 0 , sizeof(MMWave_InitCfg));

    /* Populate the init configuration for mmwave library: */               // nastaveni MMW modulu
    initCfg.domain                  = MMWave_Domain_MSS;                       // rizeno z MSS
    initCfg.socHandle               = gMmwMssMCB.socHandle;                    // oznaceni
    initCfg.eventFxn                = MmwDemo_mssMmwaveEventCallbackFxn;       // preruseni od udalosti
    initCfg.linkCRCCfg.useCRCDriver = 1U;
    initCfg.linkCRCCfg.crcChannel   = CRC_Channel_CH1;
    initCfg.cfgMode                 = MMWave_ConfigurationMode_FULL;         // MMW budeme pln� konfigurovat(chirp, frame)
    initCfg.executionMode           = MMWave_ExecutionMode_COOPERATIVE;
    initCfg.cooperativeModeCfg.cfgFxn = MmwDemo_mssMmwaveConfigCallbackFxn;
    initCfg.cooperativeModeCfg.startFxn = MmwDemo_mssMmwaveStartCallbackFxn;
    initCfg.cooperativeModeCfg.stopFxn = MmwDemo_mssMmwaveStopCallbackFxn;        
        
    /* Initialize and setup the mmWave Control module */
    // naninicializuje mmw modul ... tedy BSS
    gMmwMssMCB.ctrlHandle = MMWave_init (&initCfg, &errCode);
    if (gMmwMssMCB.ctrlHandle == NULL)
    {
        /* Error: Unable to initialize the mmWave control module */
        System_printf("Error: MMWDemoMSS mmWave Control Initialization failed [Error code %d]\n", errCode);
        return;
    }
    System_printf("Debug: MMWDemoMSS 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. */
    while (1)
    {
        int32_t syncStatus;

        /* Get the synchronization status: */
        syncStatus = MMWave_sync (gMmwMssMCB.ctrlHandle , &errCode);
        if (syncStatus < 0)
        {
            /* Error: Unable to synchronize the mmWave control module */
            System_printf ("Error: MMWDemoMSS mmWave Control Synchronization failed [Error code %d]\n", errCode);
            return;
        }
        if (syncStatus == 1)
        {
            /* Synchronization acheived: */
            break;
        }
        /* Sleep and poll again: */
        Task_sleep(1);
    }  

    /*****************************************************************************
     * 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 = 6;
    taskParams.stackSize = 3*1024;
    Task_create(MmwDemo_mmWaveCtrlTask, &taskParams, NULL);

    /*****************************************************************************
     * Create a data path management task to handle data Path events
     *****************************************************************************/
    Task_Params_init(&taskParams);
    taskParams.priority = 4;
    taskParams.stackSize = 3*1024;
    Task_create(MmwDemo_mssCtrlPathTask, &taskParams, NULL);
    
    /*Task co odesila data po UART logging*/
    Task_Params_init(&taskParams);
    taskParams.priority = 5;
    taskParams.stackSize = 3*1024;
    Task_create(Mmw_sensorReadyToSendDataLoggingTask, &taskParams, NULL);


    /* Configure task. */
    Task_Params_init(&taskParams);
    taskParams.priority = 7;
    taskParams.stackSize = 3*1024;
    Task_create(Mmw_SPIDataLoggingTask, &taskParams, NULL);
    /*****************************************************************************
     * At this point, MSS and DSS are both up and synced. Configuration is ready to be sent.
     * Start CLI to get configuration from user
     *****************************************************************************/
    MmwDemo_CLIInit();

    /*****************************************************************************
     * Benchmarking Count init
     *****************************************************************************/
    /* Configure banchmark counter */
    Pmu_configureCounter(0, 0x11, FALSE);
    Pmu_startCounter(0);

    /* int32_t err = MmwDemo_BSSinitToDefault();

    if (err >= 0)
    {
        System_printf ("DEBUG: BSS se povedlo nakonfigurovat\n");
        return;
    }
    else
    {

        System_printf ("Error: BSS se nepovedlo nakonfigurovat [Error code %d]\n", err < 0);
        return;
    } */

}

/**
 *  @b Description
 *  @n
 *      Entry point into the Millimeter Wave Demo
 *
 *  @retval
 *      Not Applicable.
 */
int main (void)
{
    Task_Params     taskParams;
    int32_t         errCode;
    SOC_Cfg         socCfg;


    /* Initialize the ESM: */
    errCode = ESM_init(0U); //dont clear errors as TI RTOS does it

    /* Initialize and populate the demo MCB */
    memset ((void*)&gMmwMssMCB, 0, sizeof(MmwDemo_MCB));

    /* Initialize the SOC confiugration: */
    memset ((void *)&socCfg, 0, sizeof(SOC_Cfg));

    /* Populate the SOC configuration: */
    socCfg.clockCfg = SOC_SysClock_INIT;

    /* Initialize the SOC Module: This is done as soon as the application is started
     * to ensure that the MPU is correctly configured. */
    gMmwMssMCB.socHandle   = SOC_init (&socCfg, &errCode);
    if (gMmwMssMCB.socHandle  == NULL)
    {
        System_printf ("Error: SOC Module Initialization failed [Error code %d]\n", errCode);
        return -1;
    }

    /* Zad�me hodiny pro jednotliv� seriov� linky : */
    gMmwMssMCB.cfg.sysClockFrequency = MSS_SYS_VCLK;
    gMmwMssMCB.cfg.loggingBaudRate   = 921600; // pouze vys�lan� .. odes�l�n� dat
    gMmwMssMCB.cfg.commandBaudRate   = 115200; // slou�� k odes�l�n� a p��jmu dat

    /* Debug Message: */
    System_printf ("**********************************************\n");
    System_printf (" Debug: senzorek MMW IWR 1642_onlinePrenosDat \n");
    System_printf ("**********************************************\n");

    /* Inicializace a spu�t�n� hlavn�ch �loh. */
    Task_Params_init(&taskParams);
    taskParams.priority = 3;
    Task_create(MmwDemo_mssInitTask, &taskParams, NULL);

    /* Start BIOS */
    BIOS_start();
    return 0;
}