www.pudn.com > ethercat-slave-source-code.rar > emcy.c, change:2013-05-03,size:10843b


/** 
\addtogroup EMCY Emergency Messages	 
@{ 
*/ 
 
/** 
\file emcy.c 
\author EthercatSSC@beckhoff.com 
\brief Implementation 
This file contains the Emergency interface 
 
\version 5.0 
 
<br>Changes to version V4.40:<br> 
V5.0 EMCY1: enable mailbox interrupt again before "EMCY_SendEmergency()" is abort.<br> 
<br>Changes to version V4.10:<br> 
V4.40 SOE1: provide MAX_EMCY_SIZE also if CoE is not supported<br> 
V4.40 EMCY1: Prevent sending empty mailbox<br> 
<br>Changes to version V3.20:<br> 
V4.10 EMCY 1: AlStatus comparison fixed 
*/ 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    Includes 
------ 
---------------------------------------------------------------------------------------*/ 
 
#include "ecat_def.h" 
 
#if EMERGENCY_SUPPORTED 
 
#include "ecatslv.h" 
 
#if COE_SUPPORTED || SOE_SUPPORTED 
 
#define    _EMCY_    1 
#include "emcy.h" 
#undef      _EMCY_ 
#define    _EMCY_    0 
 
#if COE_SUPPORTED 
#include "ecatcoe.h" 
#endif 
#if SOE_SUPPORTED 
#include "ecatsoe.h" 
#endif 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    constants 
------ 
---------------------------------------------------------------------------------------*/ 
 
#if COE_SUPPORTED 
const UINT16 MBXMEM acEmcyCoeHeader[4] = {COE_HEADER_SIZE + EMCY_SIZE, 0, SWAPWORD(MBX_TYPE_COE<<8), SWAPWORD(COESERVICE_EMERGENCY<<12)}; 
#define    MAX_EMCY_SIZE    (SIZEOF(acEmcyCoeHeader)+SIZEOF(TEMCYMESSAGE)) 
 
#endif 
#if SOE_SUPPORTED 
const UINT16 MBXMEM acEmcySoeHeader[3] = {SOE_HEADER_SIZE + EMCY_SIZE, 0, SWAPWORD(MBX_TYPE_SOE<<8)}; 
const UINT16 MBXMEM acNotiSoeHeader[3] = {SOE_HEADER_SIZE + SIZEOF(UINT16), 0, SWAPWORD(MBX_TYPE_SOE<<8)}; 
#ifndef MAX_EMCY_SIZE 
#define    MAX_EMCY_SIZE    (SIZEOF(acEmcySoeHeader)+SIZEOF(TEMCYMESSAGE)) 
#endif 
#endif 
/*--------------------------------------------------------------------------------------- 
------ 
------    Functions 
------ 
---------------------------------------------------------------------------------------*/ 
#endif 
 
 
#if COE_SUPPORTED || SOE_SUPPORTED 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pEmcy             empty emergency buffer 
 
 \brief    This function puts an emergency buffer back to the empty queue 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
void PutInEmptyEmcyQueue(TEMCYMESSAGE EMCYMEM * pEmcy) 
{ 
    UINT16 lastInQueue = sEmptyEmcyQueue.LastInQueue+1; 
 
    if (lastInQueue == sEmptyEmcyQueue.MaxQueueSize) 
    { 
        lastInQueue = 0; 
    } 
 
 
    sEmptyEmcyQueue.pQueue[sEmptyEmcyQueue.LastInQueue] = pEmcy; 
    sEmptyEmcyQueue.LastInQueue = lastInQueue; 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \return        empty emergency buffer 
 
 \brief    This function gets an emergency buffer from the empty queue 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
TEMCYMESSAGE EMCYMEM * GetOutOfEmptyEmcyQueue(void) 
{ 
    TEMCYMESSAGE EMCYMEM * pEmcy; 
 
    DISABLE_EMCY_INT; 
    if (sEmptyEmcyQueue.FirstInQueue != sEmptyEmcyQueue.LastInQueue) 
    { 
        UINT16 firstInQueue = sEmptyEmcyQueue.FirstInQueue; 
 
        pEmcy = sEmptyEmcyQueue.pQueue[firstInQueue++]; 
        sEmptyEmcyQueue.FirstInQueue = firstInQueue; 
        if (sEmptyEmcyQueue.FirstInQueue == sEmptyEmcyQueue.MaxQueueSize) 
        { 
            sEmptyEmcyQueue.FirstInQueue = 0; 
        } 
    } 
    else 
        pEmcy = 0; 
    ENABLE_EMCY_INT; 
 
    return pEmcy; 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pEmcy         full emergency buffer 
 
 \return    0: Success. 
 
 \brief    This function puts an emergency buffer in the send queue 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
void PutInSendEmcyQueue(TEMCYMESSAGE EMCYMEM * pEmcy) 
{ 
    UINT16 lastInQueue; 
 
    DISABLE_EMCY_INT; 
    lastInQueue = sSendEmcyQueue.LastInQueue+1; 
 
    if (lastInQueue == sSendEmcyQueue.MaxQueueSize) 
    { 
        lastInQueue = 0; 
    } 
 
    sSendEmcyQueue.pQueue[sSendEmcyQueue.LastInQueue] = pEmcy; 
    sSendEmcyQueue.LastInQueue = lastInQueue; 
    ENABLE_EMCY_INT; 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \return        full emergency buffer 
 
 \brief    This function gets an emergency buffer to be sent 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
TEMCYMESSAGE EMCYMEM * GetOutOfSendEmcyQueue(void) 
{ 
    TEMCYMESSAGE EMCYMEM * pEmcy; 
 
    DISABLE_EMCY_INT; 
    if (sSendEmcyQueue.FirstInQueue != sSendEmcyQueue.LastInQueue) 
    { 
        UINT16 firstInQueue = sSendEmcyQueue.FirstInQueue; 
 
        pEmcy = sSendEmcyQueue.pQueue[firstInQueue++]; 
        sSendEmcyQueue.FirstInQueue = firstInQueue; 
        if (sSendEmcyQueue.FirstInQueue == sSendEmcyQueue.MaxQueueSize) 
        { 
            sSendEmcyQueue.FirstInQueue = 0; 
        } 
    } 
    else 
        pEmcy = 0; 
    ENABLE_EMCY_INT; 
 
    return pEmcy; 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 
 \brief    This function intialize the Emergency Interface. 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
void EMCY_Init(void) 
{ 
    UINT8 i; 
 
    /* initialize send queue */ 
    sSendEmcyQueue.FirstInQueue                    = 0; 
    sSendEmcyQueue.LastInQueue                    = 0; 
    sSendEmcyQueue.PutInQueueFailedCounter    = 0; 
    /* Bug 27.07: MaxQueueSize has to be set to the number of elements */ 
    sSendEmcyQueue.MaxQueueSize                    = MAX_EMERGENCIES+1; 
    for (i = 0; i < MAX_EMERGENCIES; i++) 
    { 
        sSendEmcyQueue.pQueue[i] = 0; 
    } 
 
    /* initialize empty queue */ 
    sEmptyEmcyQueue.FirstInQueue                    = 0; 
    sEmptyEmcyQueue.LastInQueue                    = 0; 
    sEmptyEmcyQueue.PutInQueueFailedCounter    = 0; 
    /* Bug 27.07: MaxQueueSize has to be set to the number of elements */ 
    sEmptyEmcyQueue.MaxQueueSize                    = MAX_EMERGENCIES+1; 
    for (i = 0; i < MAX_EMERGENCIES; i++) 
    { 
        sEmptyEmcyQueue.pQueue[i] = 0; 
    } 
 
    /* put all buffers in empty queue */ 
    for (i = 0; i < MAX_EMERGENCIES; i++) 
        PutInEmptyEmcyQueue(&asEmcy[i]); 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \return        1 is the emergency queue is empty 
 
 \brief    This function checks if the emergency queue is empty 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT8 EMCY_IsQueueEmpty(void) 
{ 
    return (sSendEmcyQueue.FirstInQueue == sSendEmcyQueue.LastInQueue); 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pMbx      empty mailbox buffer 
 
 \brief    This function is called when the next mailbox command can be stored the ESC. 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
void EMCY_ContinueInd(TMBX MBXMEM * pMbx) 
{ 
    TEMCYMESSAGE EMCYMEM *pEmcy = GetOutOfSendEmcyQueue(); 
    if (pEmcy != NULL) 
    { 
        /* send next emergency message */ 
        EMCY_SendEmergency( pEmcy ); 
    } 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \return                    empty emergency buffer 
 
 \brief    This function gets an empty emergency buffer 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
TEMCYMESSAGE EMCYMEM * EMCY_GetEmcyBuffer(void) 
{ 
#if SOE_SUPPORTED 
    TEMCYMESSAGE EMCYMEM * pEmcy = GetOutOfEmptyEmcyQueue(); 
    pEmcy->SoeHeader[0] = SWAPWORD(ECAT_SOE_OPCODE_EMCY); 
    pEmcy->SoeHeader[1] = 0; 
 
    return pEmcy; 
#else 
    TEMCYMESSAGE EMCYMEM * pEmcy; 
 
    // HBu 02.05.06: when using the mailbox event in an ISR it should be disabled here 
    DISABLE_MBX_INT; 
    pEmcy = GetOutOfEmptyEmcyQueue(); 
    // HBu 02.05.06: when using the mailbox event in an ISR it should be enabled here 
    ENABLE_MBX_INT; 
    return pEmcy; 
#endif 
} 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pEmcy                emergency message to be sent 
 
 \brief    This function sends an emergency message 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT8 EMCY_SendEmergency( TEMCYMESSAGE EMCYMEM *pEmcy ) 
{ 
    // HBu 02.05.06: when using the mailbox event in an ISR it should be disabled here 
    DISABLE_MBX_INT; 
    if ( bSendMbxIsFull || (nAlStatus & STATE_MASK) == STATE_INIT ) 
    { 
        /* send mailbox is full store the emergency message */ 
        PutInSendEmcyQueue( pEmcy ); 
 
        u8MailboxSendReqStored |= EMCY_SERVICE; 
    // HBu 02.05.06: when using the mailbox event in an ISR it should be enabled here 
    ENABLE_MBX_INT; 
        return 0; 
    } 
 
    /* buffer psWriteMbx is free if bSendMbxIsFull is not set */ 
#if MAILBOX_QUEUE 
    psWriteMbx = (TMBX MBXMEM *) APPL_AllocMailboxBuffer(MAX_EMCY_SIZE); 
    /* it shall be checked if a valid pointer was returned */ 
    if ( psWriteMbx == NULL ) 
    { 
        ENABLE_MBX_INT; 
        return ALSTATUSCODE_NOMEMORY; 
    } 
 
#endif 
#if COE_SUPPORTED 
    OBJTOMBXMEMCPY(psWriteMbx, acEmcyCoeHeader, SIZEOF(acEmcyCoeHeader)); 
    MBXMEMCPY(&psWriteMbx->Data[1], pEmcy, SIZEOF(TEMCYMESSAGE)); 
#endif 
#if SOE_SUPPORTED 
    OBJTOMBXMEMCPY(psWriteMbx, acEmcySoeHeader, SIZEOF(acEmcySoeHeader)); 
    MBXMEMCPY(&psWriteMbx->Data[1], pEmcy, SIZEOF(TEMCYMESSAGE)); 
    if    ( (SWAPWORD(((TSOEHEADER EMCYMEM *) pEmcy->SoeHeader)->Flags.Word) & SOEFLAGS_OPCODE) == ECAT_SOE_OPCODE_NFC ) 
        OBJTOMBXMEMCPY(psWriteMbx, acNotiSoeHeader, SIZEOF(acNotiSoeHeader)); 
#if COE_SUPPORTED 
    else 
        OBJTOMBXMEMCPY(psWriteMbx, acEmcyCoeHeader, SIZEOF(acEmcySoeHeader)); 
#endif 
#endif 
 
    // HBu 02.05.06: emergency buffer has to be put in the empty queue only 
    //               if the sending was successful 
    if (MBX_MailboxSendReq(psWriteMbx, EMCY_SERVICE) == 0) 
        /* put emergency buffer back in the empty queue */ 
        PutInEmptyEmcyQueue( pEmcy ); 
    // HBu 02.05.06: when using the mailbox event in an ISR it should be enabled here 
    ENABLE_MBX_INT; 
 
    return 0; 
} 
 
 
#endif // COE_SUPPORTED || SOE_SUPPORTED 
 
/** @} */ 
#endif //#if EMERGENCY_SUPPORTED