www.pudn.com > MicroCANOPEN.rar > mco.c, change:2003-05-28,size:22529b


/************************************************************************** 
MODULE:    MCO 
CONTAINS:  MicroCANopen implementation 
COPYRIGHT: Embedded Systems Academy, Inc. 2002-2003. 
           All rights reserved. www.microcanopen.com 
           This software was written in accordance to the guidelines at 
		   www.esacademy.com/software/softwarestyleguide.pdf 
DISCLAIM:  Read and understand our disclaimer before using this code! 
           www.esacademy.com/disclaim.htm 
LICENSE:   Users that have purchased a license for PCANopenMagic 
           (www.esacademy.com/software/pcanopenmagic) 
           may use this code in commercial projects. 
           Otherwise only educational use is acceptable. 
VERSION:   1.20, Pf/Aa/Ck 28-MAY-03 
--------------------------------------------------------------------------- 
HISTORY:   1.20, Pf 28-MAY-03, Made OD an include, part of mco.h 
                 changed OD to indefinite length with end of table record 
				 added support for CANopen RUN and ERR LED 
           1.01, Pf 17-DEC-02, Made Object Dictionary more readable 
           1.00, Pf 07-OCT-02, First Published Version 
***************************************************************************/  
 
#include "mco.h" 
#include "mcohw.h" 
#include <string.h> 
 
 
/************************************************************************** 
MACROS FOR OBJECT DICTIONARY ENTRIES 
***************************************************************************/  
 
#define GETBYTE(val,pos) ((val >> pos) & 0xFF) 
#define GETBYTES16(val) GETBYTE(val, 0), GETBYTE(val, 8) 
#define GETBYTES32(val) GETBYTE(val, 0), GETBYTE(val, 8), GETBYTE(val,16), GETBYTE(val,24) 
#define SDOREPLY(index,sub,len,val) 0x43 | ((4-len)<<2), GETBYTES16(index), sub, GETBYTES32(val) 
#define SDOREPLY4(index,sub,len,d1,d2,d3,d4) 0x43 | ((4-len)<<2), GETBYTES16(index), sub, d1, d2, d3, d4 
 
 
/************************************************************************** 
GLOBAL VARIABLES 
***************************************************************************/  
 
#ifdef USE_LED 
// CAN Run and Err LED 
extern BYTE data gRLED; // Current pattern on run led 
extern BYTE data gELED; // Current pattern on error led 
#endif 
 
// This structure holds all node specific configuration 
MCO_CONFIG gMCOConfig; 
 
#if NR_OF_TPDOS > 0 
// This structure holds all the TPDO configuration data for up to 4 TPDOs 
TPDO_CONFIG gTPDOConfig[NR_OF_TPDOS]; 
 
// This is the next TPDO to be checked in MCO_ProcessStack 
BYTE gTPDONr = NR_OF_TPDOS; 
#endif 
 
#if NR_OF_RPDOS > 0 
// This structure holds all the RPDO configuration data for up to 4 RPDOs 
RPDO_CONFIG gRPDOConfig[NR_OF_RPDOS]; 
#endif 
 
// This structure holds the current receive message 
CAN_MSG data gRxCAN; 
 
// This structure holds the CAN message for SDO responses or aborts 
CAN_MSG data gTxSDO; 
 
// Table with SDO Responses for read requests to OD 
BYTE const code SDOResponseTable[] = { 
// Each Row has 8 Bytes: 
// Command Specifier for SDO Response (1 byte) 
//   bits 2+3 contain: '4'  {number of data bytes} 
// Object Dictionary Index (2 bytes, low first) 
// Object Dictionary Subindex (1 byte) 
// Data (4 bytes, lowest bytes first) 
 
// [1000h,00]: Device Type 
SDOREPLY(0x1000, 0x00, 4, OD_DEVICE_TYPE), 
 
#ifdef OD_SERIAL 
// [1018h,00]: Identity Object, Number of Entries = 4 
SDOREPLY(0x1018, 0x00, 1, 0x00000004L), 
#else 
// [1018h,00]: Identity Object, Number of Entries = 3 
SDOREPLY(0x1018, 0x00, 1, 0x00000003L), 
#endif 
 
// [1018h,01]: Identity Object, Vendor ID 
SDOREPLY(0x1018, 0x01, 4, OD_VENDOR_ID), 
 
// [1018h,02]: Identity Object, Product Code 
SDOREPLY(0x1018, 0x02, 4, OD_PRODUCT_CODE), 
 
// [1018h,03]: Identity Object, Revision 
SDOREPLY(0x1018, 0x03, 4, OD_REVISION), 
 
#ifdef OD_SERIAL 
// [1018h,04]: Identity Object, Serial 
SDOREPLY(0x1018, 0x04, 4, OD_SERIAL), 
#endif 
 
// [2018h,00]: MicroCANopen Identity Object, Number of Entries = 3 
SDOREPLY(0x2018, 0x00, 1, 0x00000003L), 
 
// [2018h,01]: MicroCANopen Identity Object, Vendor ID = 01455341, ESA Inc. 
SDOREPLY(0x2018, 0x01, 4, 0x01455341L), 
 
// [2018h,02]: MicroCANopen Identity Object, Product Code = "MCOP" 
SDOREPLY4(0x2018, 0x02, 4, 'P', 'O', 'C', 'M'), 
 
// [2018h,03]: MicroCANopen Identity Object, Revision = 1.20 
SDOREPLY(0x2018, 0x03, 4, 0x00010014L), 
 
// [0000h,00]: End of table marker 
SDOREPLY(0x0000, 0x00, 0, 0x00000000L) 
}; 
 
 
// SDO Abort Messages 
#define SDO_ABORT_UNSUPPORTED  0x06010000UL 
#define SDO_ABORT_NOT_EXISTS   0x06020000UL 
#define SDO_ABORT_READONLY     0x06010002UL 
 
 
 
/************************************************************************** 
LOCAL FUNCTIONS 
***************************************************************************/  
 
 
/************************************************************************** 
DOES: Search the SDO Response table for a specifc index and subindex. 
**************************************************************************/ 
BYTE Search_OD 
  (             // RETURNS: 255 if not found 
                //          number of record (starting at 0) if found 
  WORD index,   // Index of OD entry searched 
  BYTE subindex // Subindex of OD entry searched  
  ) 
{ 
BYTE data i; 
BYTE data i_hi, hi; 
BYTE data i_lo, lo; 
BYTE const code *p; 
BYTE const code *r; 
 
  i = 0; 
  i_hi = (BYTE) (index >> 8); 
  i_lo = (BYTE) index; 
  r = &(SDOResponseTable[0]); 
  while (i < 255) 
  { 
    p = r; 
    r += 8; // Set r to next record in table 
    p++; // Skip command byte 
	lo = *p; 
	p++; 
	hi = *p; 
    if ((lo == 0) && (hi == 0)) 
    { // if index in table is zero, this is end of table 
	  return 255; 
	} 
    if (lo == i_lo) 
    {  
      if (hi == i_hi) 
      {  
        p++; 
        if (*p == subindex) 
        { // Entry found 
          return i; 
        } 
      } 
    } 
    i++; 
  } // while search loop 
  return 255; 
} 
 
/************************************************************************** 
DOES: Generates an SDO Abort Response 
**************************************************************************/ 
void Send_SDO_Abort 
  ( 
  DWORD ErrorCode   // 4 byte SDO abort error code 
  ) 
{ 
BYTE i; 
 
  gTxSDO.BUF[0] = 0x80; 
  for (i=0;i<4;i++) 
  { 
    gTxSDO.BUF[4+i] = ErrorCode; 
    ErrorCode >>= 8; 
  } 
   
  if (!MCOHW_PushMessage(&gTxSDO)) 
  { 
    MCOUSER_FatalError(0x8801); 
  } 
} 
 
 
/************************************************************************** 
DOES: Copies 8 bytes from "const code" memory to "data" memory 
**************************************************************************/ 
void CopyConsToData  
  ( 
  BYTE data *dest, // Destination pointer 
  BYTE const code *src // Source pointer 
  ) 
{ 
BYTE i; 
 
  for (i = 0; i < 8; i++) 
  { 
    *dest = *src; 
	dest++; 
	src++; 
  } 
} 
 
/************************************************************************** 
DOES: Handle an incoimg SDO request. 
**************************************************************************/ 
BYTE Handle_SDO_Request  
  (               // Returns 1 if SDO Access success 
                  // Returns 0 if SDO Abort generated 
  BYTE *pData     // Pointer to 8 data bytes with SDO data 
  ) 
{ 
BYTE cmd;         // Command byte of SDO request 
WORD index;       // Index of SDO request 
BYTE subindex;    // Subindex of SDO request 
BYTE found;       // Search result of Search_OD 
 
  // Init variables 
  cmd = *pData & 0xE0; // Upper 3-bits are the command 
  index = pData[2]; // Get hi byte of index 
  index = (index << 8) + pData[1]; // Add lo byte of index 
  subindex = pData[3]; 
 
  // Copy Multiplexor into response 
  gTxSDO.BUF[1] = pData[1]; // index lo 
  gTxSDO.BUF[2] = pData[2]; // index hi 
  gTxSDO.BUF[3] = pData[3]; // subindex 
 
  if ((cmd == 0x40) || (cmd == 0x20))  
  { // It is a read or write command 
 
#ifdef DYNAMIC_HEARTBEAT 
    // Hard coding of dynamic read/write accesses 
    if ((index == 0x1017) && (subindex == 0x00)) 
    { // Accesse to [1017,00] - heartbeat time 
      if (cmd == 0x40) 
      { // Read command 
        gTxSDO.BUF[0] = 0x4B; // Expedited, 2-bytes of data 
        gTxSDO.BUF[4] = (BYTE) gMCOConfig.heartbeat_time; 
        gTxSDO.BUF[5] = (BYTE) (gMCOConfig.heartbeat_time >> 8); 
        if (!MCOHW_PushMessage(&gTxSDO)) 
        { 
          MCOUSER_FatalError(0x8802); 
        } 
        return 1; 
      } 
      if (*pData == 0x2B) 
      { // Expedited write command with 2-bytes of data 
        gMCOConfig.heartbeat_time = pData[5]; 
        gMCOConfig.heartbeat_time = (gMCOConfig.heartbeat_time << 8) + pData[4]; 
        gTxSDO.BUF[0] = 0x60; // Write response 
        if (!MCOHW_PushMessage(&gTxSDO)) 
        { 
          MCOUSER_FatalError(0x8802); 
        } 
        return 1; 
      } 
      Send_SDO_Abort(SDO_ABORT_UNSUPPORTED); 
      return 0; 
    } 
#endif // DYNAMIC HEARTBEAT 
 
    // See if the data requested is in the OD 
    found = Search_OD(index,subindex); 
    if (found < 255) 
    { // OD entry found 
      if (cmd == 0x40) 
      { // Read command 
        CopyConsToData(&gTxSDO.BUF[0],&SDOResponseTable[(found*8)]); 
        if (!MCOHW_PushMessage(&gTxSDO)) 
        { 
          MCOUSER_FatalError(0x8802); 
        } 
        return 1; 
      } 
      // Write command 
      Send_SDO_Abort(SDO_ABORT_READONLY); 
      return 0; 
    } 
    if ((index == 0x1001) && (subindex == 0x00)) 
    { 
      if (cmd == 0x40) 
      { // Read command 
        gTxSDO.BUF[0] = 0x4F; // Expedited, 1-byte of data 
        gTxSDO.BUF[4] = gMCOConfig.error_register; 
        if (!MCOHW_PushMessage(&gTxSDO)) 
        { 
          MCOUSER_FatalError(0x8802); 
        } 
        return 1; 
      } 
      // Write command 
      Send_SDO_Abort(SDO_ABORT_READONLY); 
      return 0; 
    } 
 
    Send_SDO_Abort(SDO_ABORT_NOT_EXISTS); 
    return 0; 
  } 
  if (cmd != 0x80) 
  { // Ignore "abort received" - all others produce error 
    Send_SDO_Abort(SDO_ABORT_UNSUPPORTED); 
    return 0; 
  } 
  return 1; 
} 
 
 
#if NR_OF_TPDOS > 0 
/************************************************************************** 
DOES: Called when going into the operational mode. 
      Prepares all TPDOs for operational. 
**************************************************************************/ 
void Prepare_TPDOs  
  ( 
    void 
  ) 
{ 
BYTE i; 
 
  i = 0; 
  while (i < NR_OF_TPDOS) 
  { // Prepare all TPDOs for transmission 
    if (gTPDOConfig[i].CAN.ID != 0) 
    { // This TPDO is used 
      // Copy current process data 
      memcpy(&gTPDOConfig[i].CAN.BUF[0],gTPDOConfig[i].pData,gTPDOConfig[i].CAN.LEN); 
#ifdef USE_EVENT_TIME 
      // Reset event timer for immediate transmission 
      gTPDOConfig[i].event_timestamp = MCOHW_GetTime() - 2; 
#endif 
#ifdef USE_INHIBIT_TIME 
      gTPDOConfig[i].inhibit_status = 2; // Mark as ready for transmission 
      // Reset inhibit timer for immediate transmission 
      gTPDOConfig[i].inhibit_timestamp = MCOHW_GetTime() - 2; 
#endif 
    } 
  i++; 
  } 
  gTPDONr = NR_OF_TPDOS; // Ensure that MCO_ProcessStack starts with TPDO1 
} 
 
 
/************************************************************************** 
DOES: Called when a TPDO needs to be transmitted 
**************************************************************************/ 
void TransmitPDO  
  ( 
  BYTE PDONr 
  ) 
{ 
#ifdef USE_INHIBIT_TIME 
  gTPDOConfig[PDONr].inhibit_status = 1; // New inhibit timer started 
  gTPDOConfig[PDONr].inhibit_timestamp = MCOHW_GetTime() + gTPDOConfig[PDONr].inhibit_time; 
#endif 
#ifdef USE_EVENT_TIME 
  gTPDOConfig[gTPDONr].event_timestamp = MCOHW_GetTime() + gTPDOConfig[gTPDONr].event_time;  
#endif // USE_EVENT_TIME 
  if (!MCOHW_PushMessage(&gTPDOConfig[PDONr].CAN)) 
  { 
    MCOUSER_FatalError(0x8801); 
  } 
} 
#endif // NR_OF_TPDOS > 0 
 
 
/************************************************************************** 
PUBLIC FUNCTIONS 
***************************************************************************/  
 
void MCO_Init  
  ( 
  WORD Baudrate, 
  BYTE Node_ID, 
  WORD Heartbeat 
  ) 
{ 
BYTE i; 
 
  // Init the global variables 
  gMCOConfig.Node_ID = Node_ID; 
  gMCOConfig.error_code = 0; 
  gMCOConfig.Baudrate = Baudrate; 
  gMCOConfig.heartbeat_time = Heartbeat; 
  gMCOConfig.heartbeat_msg.ID = 0x700+Node_ID; 
  gMCOConfig.heartbeat_msg.LEN = 1; 
  gMCOConfig.heartbeat_msg.BUF[0] = 0; // Current NMT state of this node = bootup 
  gMCOConfig.error_register = 0; 
  
  // Init SDO Response/Abort message 
  gTxSDO.ID = 0x580+gMCOConfig.Node_ID; 
  gTxSDO.LEN = 8; 
    
#if NR_OF_TPDOS > 0 
  i = 0; 
  while (i < NR_OF_TPDOS) 
  { // Init TPDOs 
     gTPDOConfig[i].CAN.ID = 0; 
     i++; 
  } 
#endif 
#if NR_OF_RPDOS > 0 
  i = 0; 
  while (i < NR_OF_RPDOS) 
  { // Init RPDOs 
     gRPDOConfig[i].CANID = 0; 
     i++; 
  } 
#endif 
 
  // Init the CAN interface 
  if (!MCOHW_Init(Baudrate)) 
  { 
    MCOUSER_FatalError(0x8802); 
  } 
  if (!MCOHW_SetCANFilter(0)) // For NMT master message  
  { 
    MCOUSER_FatalError(0x8803); 
  } 
  if (!MCOHW_SetCANFilter(0x600+Node_ID)) // For SDO Requests 
  { 
    MCOUSER_FatalError(0x8803); 
  } 
 
  // Signal to MCO_ProcessStack: we just initialized 
  gTPDONr = 0xFF; 
}   
 
#if NR_OF_RPDOS > 0 
/************************************************************************** 
DOES: This function initializes a receive PDO. Once initialized, the  
      MicroCANopen stack automatically updates the data at *dat. 
NOTE: For data consistency, the application should not read the data 
      while function MCO_ProcessStack executes. 
**************************************************************************/ 
void MCO_InitRPDO 
  ( 
  BYTE PDO_NR,     // RPDO number (1-4) 
  WORD CAN_ID,     // CAN identifier to be used (set to 0 to use default) 
  BYTE len,        // Number of data bytes in RPDO 
  BYTE *dat        // Pointer to destination location of data bytes 
  ) 
{ 
 
#ifdef CHECK_PARAMETERS 
  if ( ((PDO_NR < 1) || (PDO_NR > NR_OF_RPDOS)) ||  // Check PDO range  
       ((gMCOConfig.Node_ID < 1) || (gMCOConfig.Node_ID > 127)) )    
                                                    // Check Node ID range 1-127 
  { 
    MCOUSER_FatalError(0x8804); 
  } 
#endif 
  PDO_NR--; 
  gRPDOConfig[PDO_NR].LEN = len; 
  gRPDOConfig[PDO_NR].DAT = dat; 
  if (CAN_ID == 0) 
  { 
    gRPDOConfig[PDO_NR].CANID = 0x200 + (0x100 * ((WORD)(PDO_NR))) + gMCOConfig.Node_ID; 
  } 
  else 
  { 
    gRPDOConfig[PDO_NR].CANID = CAN_ID; 
  } 
  if (!MCOHW_SetCANFilter(gRPDOConfig[PDO_NR].CANID)) 
  { 
    MCOUSER_FatalError(0x8805); 
  } 
} 
#endif // NR_OF_RPDOS > 0 
 
 
#if NR_OF_TPDOS > 0 
/************************************************************************** 
DOES: This function initializes a transmit PDO. Once initialized, the  
      MicroCANopen stack automatically handles transmitting the PDO. 
      The application can directly change the data at any time. 
NOTE: For data consistency, the application should not write to the data 
      while function MCO_ProcessStack executes. 
**************************************************************************/ 
void MCO_InitTPDO 
  ( 
  BYTE PDO_NR,      // TPDO number (1-4) 
  WORD CAN_ID,      // CAN identifier to be used (set to 0 to use default) 
  WORD event_time,  // Transmitted every event_tim ms  
  WORD inhibit_time,// Inhibit time in ms for change-of-state transmit 
                    // (set to 0 if ONLY event_tim should be used) 
  BYTE len,         // Number of data bytes in TPDO 
  BYTE *pDat        // Pointer to transmit data bytes 
  ) 
{ 
 
#ifdef CHECK_PARAMETERS 
  if ( ((PDO_NR < 1) || (PDO_NR > NR_OF_TPDOS)) ||   // Check PDO range 
       ((gMCOConfig.Node_ID < 1) || (gMCOConfig.Node_ID > 127)) || // Check Node ID 
       ((len < 1) || (len > 8)) ||                   // Check len range 1-8 
       ((event_time == 0) && (inhibit_time == 0)) )  // One of these needs to be set 
  { 
    MCOUSER_FatalError(0x8806); 
  } 
#endif 
  PDO_NR--; 
  if (CAN_ID == 0) 
  { 
    gTPDOConfig[PDO_NR].CAN.ID = 0x180 + (0x100 * ((WORD)(PDO_NR))) + gMCOConfig.Node_ID; 
  } 
  else 
  { 
    gTPDOConfig[PDO_NR].CAN.ID = CAN_ID; 
  } 
  gTPDOConfig[PDO_NR].CAN.LEN = len; 
  gTPDOConfig[PDO_NR].pData = pDat; 
#ifdef USE_EVENT_TIME 
  gTPDOConfig[PDO_NR].event_time = event_time; 
#endif 
#ifdef USE_INHIBIT_TIME 
  gTPDOConfig[PDO_NR].inhibit_time = inhibit_time; 
#endif 
} 
#endif // NR_OF_TPDOS > 0 
 
 
/************************************************************************** 
DOES: This function implements the main MicroCANopen protocol stack.  
      It must be called frequently to ensure proper operation of the 
      communication stack.  
      Typically it is called from the while(1) loop in main. 
**************************************************************************/ 
BYTE MCO_ProcessStack 
  ( // Returns 0 if nothing was done 
    // Returns 1 if a CAN message was received or sent 
  void 
  ) 
{ 
BYTE i; 
BYTE ret_val = 0; 
 
  // Check if this is right after boot-up 
  if (gTPDONr == 0xFF) // Was set by MCO_Init 
  { 
    // Init heartbeat time 
    gMCOConfig.heartbeat_timestamp = MCOHW_GetTime() + gMCOConfig.heartbeat_time; 
    // Send Boot-up message   
    if (!MCOHW_PushMessage(&gMCOConfig.heartbeat_msg)) 
    { 
      MCOUSER_FatalError(0x8801); 
    } 
#ifdef AUTOSTART 
    gMCOConfig.heartbeat_msg.BUF[0] = 0x05; // going into operational state 
#ifdef USE_LED 
    gRLED = LED_ON; 
	gELED = LED_OFF; 
#endif // USE_LED 
#if NR_OF_TPDOS > 0 
    Prepare_TPDOs(); 
#endif // NR_OF_TPDOS 
#else // AUTOSTART 
    gMCOConfig.heartbeat_msg.BUF[0] = 0x7F; // going into pre-operational state 
#ifdef USE_LED 
    gRLED = LED_BLINK; 
	gELED = LED_OFF; 
#endif // USE_LED 
#endif // AUTOSTART 
    gTPDONr = NR_OF_TPDOS; // Retun value to default 
    return 1; 
  } 
  
  // Work on next incoming messages 
  if (MCOHW_PullMessage(&gRxCAN)) 
  { // Message received 
   
    // Check if it is NMT master message 
    if (gRxCAN.ID == 0) 
    { // NMT Master Message 
      if ((gRxCAN.BUF[1] == gMCOConfig.Node_ID) || (gRxCAN.BUF[1] == 0)) 
      { // NMT message is for this node or all nodes 
        switch (gRxCAN.BUF[0]) 
        { 
        case 1: // Start node 
          gMCOConfig.heartbeat_msg.BUF[0] = 5; 
#if NR_OF_TPDOS > 0           
          Prepare_TPDOs(); 
#endif 
#ifdef USE_LED 
          gRLED = LED_ON; 
          gELED = LED_OFF; 
#endif 
          break; 
        case 2: // Stop node 
          gMCOConfig.heartbeat_msg.BUF[0] = 4; 
#ifdef USE_LED 
          gRLED = LED_FLASH1; 
          gELED = LED_OFF; 
#endif 
          break; 
        case 128: 
          gMCOConfig.heartbeat_msg.BUF[0] = 127; 
#ifdef USE_LED 
          gRLED = LED_BLINK; 
          gELED = LED_OFF; 
#endif 
          break; 
        case 129: 
          // Node Reset 
          MCOUSER_ResetApplication(); 
          break; 
        case 130: 
          // Reset Communication 
          MCOUSER_ResetCommunication(); 
          break; 
        default: 
          break; 
        } // switch 
        return 1; 
      } // NMT message addressed to this node 
    } // NMT master message received 
     
    // Check if it is SDO request message 
    if (gMCOConfig.heartbeat_msg.BUF[0] != 4) 
    { // Node is NOT stopped 
      if (gRxCAN.ID == gMCOConfig.Node_ID+0x600) 
      { // SDO Request 
        i = Handle_SDO_Request(&gRxCAN.BUF[0]); // Return value not used in this version 
        return 1; 
      } 
    } 
 
#if NR_OF_RPDOS > 0 
    // Check if it is RPDO message 
    if (gMCOConfig.heartbeat_msg.BUF[0] == 5) 
    { // Node is in operational 
      i = 0; 
      while (i < NR_OF_RPDOS) 
      { 
        if (gRxCAN.ID == gRPDOConfig[i].CANID) 
        { // RPDO match 
          memcpy(gRPDOConfig[i].DAT,&(gRxCAN.BUF[0]),gRPDOConfig[i].LEN); 
          i = NR_OF_RPDOS; 
          ret_val = 1; 
        } 
        i++; 
      } // for all RPDOs 
    } // Node is operational 
#endif // NR_OF_RPDOS > 0 
  } // Message received 
 
#if NR_OF_TPDOS > 0 
  // Check Next TPDO for transmit 
  if (gMCOConfig.heartbeat_msg.BUF[0] == 0x05) 
  { // Node is in operational 
    gTPDONr++; 
    if (gTPDONr >= NR_OF_TPDOS) 
    { 
      gTPDONr = 0; 
    } 
    if (gTPDOConfig[gTPDONr].CAN.ID != 0) 
    { // TPDO 'gTPDONr' is used 
#ifdef USE_EVENT_TIME 
      if ((gTPDOConfig[gTPDONr].event_time != 0) &&  
          (MCOHW_IsTimeExpired(gTPDOConfig[gTPDONr].event_timestamp)) ) 
      { // TPDO 'i' uses event timer and event timer is expired 
        // Get application data 
        memcpy(&(gTPDOConfig[gTPDONr].CAN.BUF[0]),gTPDOConfig[gTPDONr].pData,gTPDOConfig[gTPDONr].CAN.LEN); 
        TransmitPDO(gTPDONr); 
        return 1; 
      } 
#endif // USE_EVENT_TIME 
#ifdef USE_INHIBIT_TIME 
      if (gTPDOConfig[gTPDONr].inhibit_time != 0) 
      { 
        // Not using event timer, COS transmission 
        if (gTPDOConfig[gTPDONr].inhibit_status == 2) 
        { // New transmit message already waiting 
          if (MCOHW_IsTimeExpired(gTPDOConfig[gTPDONr].inhibit_timestamp)) 
          { // Inhibit time did expire 
            TransmitPDO(gTPDONr); 
            return 1; 
          } 
        } 
        else  
        { // Inhibit status is 0 or 1 
          // Check if application data changed 
          if ((memcmp(&gTPDOConfig[gTPDONr].CAN.BUF[0],gTPDOConfig[gTPDONr].pData,gTPDOConfig[gTPDONr].CAN.LEN) != 0)) 
          { // Application data changed! 
            // Copy application data 
            memcpy(&gTPDOConfig[gTPDONr].CAN.BUF[0],gTPDOConfig[gTPDONr].pData,gTPDOConfig[gTPDONr].CAN.LEN); 
            if (gTPDOConfig[gTPDONr].inhibit_status == 0) 
            { // Inhibit time already expired 
              TransmitPDO(gTPDONr); 
              return 1; 
            } 
            // inhibit_status is 1  
            gTPDOConfig[gTPDONr].inhibit_status = 2; // Wait for inhibit time to expire 
          } 
        } 
      } // Inhibit Time != 0 
#endif // USE_INHIBIT_TIME 
    } // PDO active (CAN_ID != 0)   
  } // if node is operational 
#endif // NR_OF_TPDOS > 0 
   
  // Produce Heartbeat 
  if (gMCOConfig.heartbeat_time != 0) 
  { 
    if (MCOHW_IsTimeExpired(gMCOConfig.heartbeat_timestamp)) 
    { 
      if (!MCOHW_PushMessage(&gMCOConfig.heartbeat_msg)) 
      { 
        MCOUSER_FatalError(0x8801); 
      } 
      gMCOConfig.heartbeat_timestamp = MCOHW_GetTime() + gMCOConfig.heartbeat_time; 
      ret_val = 1; 
    } 
  } 
  return ret_val; 
}