RTOS/AM4379: EtherCAT network start up error

/**
 \defgroup tiescappl tiescappl.c TI ESC Application
\brief V5.0 : create file 


\version 5.0
*/
/**
 * tiescappl.c
 * 
 *
 * Copyright (c) 2012 Texas Instruments Incorporated ALL RIGHTS RESERVED
 *
*/
//---------------------------------------------------------------------------------------
/**
\ingroup tiescappl
\file tiescappl.c
\brief Implementation.
*/
//---------------------------------------------------------------------------------------

/*-----------------------------------------------------------------------------------------
------
------    Includes
------
-----------------------------------------------------------------------------------------*/
#include "ecat_def.h"

#if TIESC_APPLICATION

#if COE_SUPPORTED
#include "objdef.h"
#endif
#include "ecatappl.h"

#include "tiescutils.h"

#define _TIESC_HW_
#include "ecatslv.h"
#include "tiescappl.h"
#undef _TIESC_HW_

#ifdef AM43XX_FAMILY_BUILD
#include "endat_drv.h"

#include <IQmathLib.h>
#endif

extern boardId_t boardType;

/*--------------------------------------------------------------------------------------
------
------    local types and defines
------
--------------------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------------------
------
------    local variables and constants
------
-----------------------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------------------
------
------    application specific functions
------
-----------------------------------------------------------------------------------------*/
extern Uint32 appState;
/*-----------------------------------------------------------------------------------------
------
------    generic functions
------
-----------------------------------------------------------------------------------------*/
/////////////////////////////////////////////////////////////////////////////////////////
/**

\brief This function resets the outputs
*////////////////////////////////////////////////////////////////////////////////////////

void PDO_ResetOutputs(void)
{
    sDOOutputs.LEDs = 0x00;
    sDO1Outputs.Cmd = 0x00;
    sDO1Outputs.Count = 0x00;
    sDO1Outputs.MotorData = 0x1000;
    sAIMyOutputObject.aValue=0x10000;
    sAIMyOutputObject.aValue_1=0x1000;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \brief    The function is called when an error state was acknowledged by the master

*////////////////////////////////////////////////////////////////////////////////////////

void    APPL_AckErrorInd(Uint16 stateTrans)
{

}
/*ECATCHANGE_START(V4.42.1) ECAT2*/
/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return    AL Status Code (see ecatslv.h ALSTATUSCODE_....)

 \brief    The function is called in the state transition from INIT to PREOP when
             all general settings were checked to start the mailbox handler. This function
             informs the application about the state transition, the application can refuse
             the state transition when returning an AL Status error code.
            The return code NOERROR_INWORK can be used, if the application cannot confirm
            the state transition immediately, in that case this function will be called cyclically
            until a value unequal NOERROR_INWORK is returned

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StartMailboxHandler(void)
{
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return     0, NOERROR_INWORK

 \brief    The function is called in the state transition from PREEOP to INIT
             to stop the mailbox handler. This functions informs the application
             about the state transition, the application cannot refuse
             the state transition.

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StopMailboxHandler(void)
{
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \param    pIntMask    pointer to the AL Event Mask which will be written to the AL event Mask
                        register (0x204) when this function is succeeded. The event mask can be adapted
                        in this function
 \return    AL Status Code (see ecatslv.h ALSTATUSCODE_....)

 \brief    The function is called in the state transition from PREOP to SAFEOP when
             all general settings were checked to start the input handler. This function
             informs the application about the state transition, the application can refuse
             the state transition when returning an AL Status error code.
             When returning ALSTATUSCODE_NOERROR, the inputs has to be updated once before return.
            The return code NOERROR_INWORK can be used, if the application cannot confirm
            the state transition immediately, in that case the application need to be complete 
            the transition by calling ECAT_StateChange
*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StartInputHandler(Uint16 *pIntMask)
{
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return     0, NOERROR_INWORK

 \brief    The function is called in the state transition from SAFEOP to PREEOP
             to stop the input handler. This functions informs the application
             about the state transition, the application cannot refuse
             the state transition.

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StopInputHandler(void)
{
#if ESC_EEPROM_EMULATION
    if(BOARD_ICEV2 == boardType)
    {
        /*
            Since both SPI Flash and HVS (input) are on the same SPI instance, application will have to make sure that 
            access to both does not happen concurrently. At this context, it is ensured that input(HVS) read wont happen.
        */
        if(bsp_get_eeprom_update_status())
        {
            bsp_set_eeprom_update_status(0);
            bsp_eeprom_emulation_flush();    
        }
    }
#endif    
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return    AL Status Code (see ecatslv.h ALSTATUSCODE_....)

 \brief    The function is called in the state transition from SAFEOP to OP when
             all general settings were checked to start the output handler. This function
             informs the application about the state transition, the application can refuse
             the state transition when returning an AL Status error code.
            The return code NOERROR_INWORK can be used, if the application cannot confirm
            the state transition immediately, in that case this function will be called cyclically
            until a value unequal NOERROR_INWORK is returned
*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StartOutputHandler(void)
{
    /*always return NOERROR_INWORK the state transition will be finished by calling AL_ControlRes*/
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return     0, NOERROR_INWORK

 \brief    The function is called in the state transition from OP to SAFEOP
             to stop the output handler. This functions informs the application
             about the state transition, the application cannot refuse
             the state transition.

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StopOutputHandler(void)
{
    PDO_ResetOutputs();
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
\return     0(ALSTATUSCODE_NOERROR), NOERROR_INWORK
\param      pInputSize  pointer to save the input process data length
\param      pOutputSize  pointer to save the output process data length

\brief    This function calculates the process data sizes from the actual SM-PDO-Assign
            and PDO mapping
*////////////////////////////////////////////////////////////////////////////////////////
Uint16 APPL_GenerateMapping(Uint16* pInputSize,Uint16* pOutputSize)
{
    Uint16 result = 0;
    Uint16 PDOAssignEntryCnt = 0;
    OBJCONST TOBJECT OBJMEM * pPDO = NULL;
    Uint16 PDOSubindex0 = 0;
    UINT32 *pPDOEntry = NULL;
    Uint16 PDOEntryCnt = 0;
    Uint16 InputSize = 0;
    Uint16 OutputSize = 0;

    /*Scan object 0x1C12 RXPDO assign*/
    for(PDOAssignEntryCnt = 0; PDOAssignEntryCnt < sRxPDOassign.u16SubIndex0; PDOAssignEntryCnt++)
    {
        pPDO = OBJ_GetObjectHandle(sRxPDOassign.aEntries[PDOAssignEntryCnt]);
        if(pPDO != NULL)
        {
            PDOSubindex0 = *((Uint16 *)pPDO->pVarPtr);
            for(PDOEntryCnt = 0; PDOEntryCnt < PDOSubindex0; PDOEntryCnt++)
            {
                pPDOEntry = (UINT32 *)((Uint8 *)pPDO->pVarPtr + (OBJ_GetEntryOffset((PDOEntryCnt+1),pPDO)>>3));        //goto PDO entry
                // we increment the expected output size depending on the mapped Entry 
                OutputSize += (Uint16) ((*pPDOEntry) & 0xFF);
            }
        }
    }
        
    OutputSize = (OutputSize + 7) >> 3;

    /*Scan Object 0x1C13 TXPDO assign*/
    for(PDOAssignEntryCnt = 0; PDOAssignEntryCnt < sTxPDOassign.u16SubIndex0; PDOAssignEntryCnt++)
    {
        pPDO = OBJ_GetObjectHandle(sTxPDOassign.aEntries[PDOAssignEntryCnt]);
        if(pPDO != NULL)
        {
            PDOSubindex0 = *((Uint16 *)pPDO->pVarPtr);
            for(PDOEntryCnt = 0; PDOEntryCnt < PDOSubindex0; PDOEntryCnt++)
            {
                pPDOEntry = (UINT32 *)((Uint8 *)pPDO->pVarPtr + (OBJ_GetEntryOffset((PDOEntryCnt+1),pPDO)>>3));        //goto PDO entry
                // we increment the expected output size depending on the mapped Entry 
                InputSize += (Uint16) ((*pPDOEntry) & 0xFF);
            }
        }
    }
    InputSize = (InputSize + 7) >> 3;

    *pInputSize = InputSize;
    *pOutputSize = OutputSize;

    CONSOLEUtilsPrintf("\nInputSize = %d \nOutput Size = %d\n ", InputSize,OutputSize);

    return result;
}


/////////////////////////////////////////////////////////////////////////////////////////
/**
\param      pData  pointer to input process data
\brief      This function will copies the inputs from the local memory to the ESC memory
            to the hardware
*////////////////////////////////////////////////////////////////////////////////////////
void APPL_InputMapping(Uint16* pData)
{
    Uint16 j = 0;
    Uint8 *pTmpData = (Uint8 *)pData;


   for (j = 0; j < sTxPDOassign.u16SubIndex0; j++)
   {


       switch (sTxPDOassign.aEntries[j])
      {
      /* TxPDO 1 */
      case 0x1A00:


         *pTmpData++ = sDIInputs.switchs;
         break;
         /* TxPDO 2 */
      case 0x1A03: // attention: maybe a non-aligned byte access...


         *pTmpData++ = sAI1Inputs.info1 & 0xFF;
         *pTmpData++ = (sAI1Inputs.info1 & 0xFF00) >> 8;
         *pTmpData++ = (sAI1Inputs.info1 & 0xFF0000) >> 16;
         *pTmpData++ = (sAI1Inputs.info1 & 0xFF000000) >> 24;
         *pTmpData++ = sAI1Inputs.info2 & 0xFF;
         *pTmpData++ = (sAI1Inputs.info2 & 0xFF00) >> 8;
         *pTmpData++ = sAI1Inputs.New & 0xFF;
         *pTmpData++ = (sAI1Inputs.New & 0xFF00) >> 8;
         break;

      case 0x1A0F:
          CONSOLEUtilsPrintf("\nInside 1A0F");
          *pTmpData++ = sNewInputs.new_input & 0xFF;
          *pTmpData++ = (sNewInputs.new_input & 0xFF00) >> 8;
          *pTmpData++ = (sNewInputs.new_input & 0xFF0000) >> 16;
          *pTmpData++ = (sNewInputs.new_input & 0xFF000000) >> 24;
          *pTmpData++ = sNewInputs.new_test & 0xFF;
          *pTmpData++ = (sNewInputs.new_test & 0xFF00) >> 8;
          break;
      }
   }
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
\param      pData  pointer to output process data

\brief    This function will copies the outputs from the ESC memory to the local memory
            to the hardware
*////////////////////////////////////////////////////////////////////////////////////////
void APPL_OutputMapping(Uint16* pData)
{
    Uint16 j = 0;
    Uint8 *pTmpData = (Uint8 *)pData;// allow byte processing

    for (j = 0; j < sRxPDOassign.u16SubIndex0; j++)
    {
        switch (sRxPDOassign.aEntries[j])
        {
        /* RxPDO 2 */
        case 0x1601:
            sDOOutputs.LEDs = *pTmpData++;
            break;
        case 0x1602:
            sDO1Outputs.Count = *pTmpData++;
            sDO1Outputs.Cmd   = *pTmpData++;
            sDO1Outputs.MotorData = *pTmpData++;
            sDO1Outputs.MotorData |= (*pTmpData++ << 8);
            break;
        case 0x1603 :
            sAIMyOutputObject.aValue   = *pTmpData++;
            sAIMyOutputObject.aValue  |=  (*pTmpData++ << 8 );
            sAIMyOutputObject.aValue  |=  (*pTmpData++ << 16);
            sAIMyOutputObject.aValue  |=  (*pTmpData++ << 24);
            sAIMyOutputObject.aValue_1   = *pTmpData++;
            sAIMyOutputObject.aValue_1   |= (*pTmpData++ << 8);
            break;
        }
    }
}
#ifdef AM43XX_FAMILY_BUILD
/////////////////////////////////////////////////////////////////////////////////////////
/**
\brief    This function will called from the synchronisation ISR 
            or from the mainloop if no synchronisation is supported
*////////////////////////////////////////////////////////////////////////////////////////
extern volatile int ECAT_ChannelSel;
extern volatile _iq ECAT_SpeedRef;
extern volatile _iq ECAT_lsw;

#endif
extern PRUICSS_Handle pruIcss1Handle;

int foc_get_angle(void);

void APPL_Application(void)
{
    Uint8 LED; // initial test data
   // static Uint8 prevState = 55;
    

    LED = sDOOutputs.LEDs;

   // CONSOLEUtilsPrintf("\nMy Object  %d \n\t ",sAIMyObject.aVariable);


    CONSOLEUtilsPrintf("\nMy RxPDO Object  %d \n\t ",sAIMyOutputObject.aValue );

    sNewInputs.new_input = sAIMyOutputObject.aValue;

    CONSOLEUtilsPrintf("\nMy TxPDO Object  %d \n\t ",sNewInputs.new_input );

    //(LED != prevState)
    	Board_setDigOutput(sAIMyOutputObject.aValue);
   // prevState = LED;


    	appState = sDO1Outputs.Cmd;// set the application state


    Board_getDigInput(&sDIInputs.switchs);
    if(appState == 0)
        appState = sDIInputs.switchs;//special mode to control app state by input switchs!
#ifdef AM43XX_FAMILY_BUILD
    sAI1Inputs.info1 = foc_get_angle();

    // Motor / FOC exchange

    if((sDO1Outputs.Cmd>=0) && (sDO1Outputs.Cmd <=2))
    {
    	ECAT_lsw = sDO1Outputs.Cmd;
    }
    else
    {
    	ECAT_lsw = 0;
    }


    if((sDO1Outputs.MotorData>=-1500) && (sDO1Outputs.MotorData<=1500))
    {
    	ECAT_SpeedRef = _IQ(((float) sDO1Outputs.MotorData) / 1000);
    }else
    {
    	ECAT_SpeedRef = 0;
    }

    ECAT_ChannelSel = sDO1Outputs.Count;
#elif defined(AM335X_FAMILY_BUILD)
    sAI1Inputs.info1 = 0x12345600 | LED;
    sAI1Inputs.info2 = bsp_read_word(pruIcss1Handle, 0x10);
#endif

}

#if EXPLICIT_DEVICE_ID
/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return    The Explicit Device ID of the EtherCAT slave

 \brief     Calculate the Explicit Device ID
*////////////////////////////////////////////////////////////////////////////////////////
Uint16 APPL_GetDeviceID()
{
    return 0xBABA;
}
#endif

/////////////////////////////////////////////////////////////////////////////////////////
/**

 \brief    This is the main function

*////////////////////////////////////////////////////////////////////////////////////////
int main()
{
    common_main();
    return 0;
}
/*ECATCHANGE_END(V4.42.1) ECAT2*/
#endif //#if TIESC_HW

/** @} */