mco.c

Canopen 最小实现源码, 只支持最小的CANOPEN功能

  gMCOConfig.heartbeat_msg.LEN = 1;
  // current NMT state of this node = bootup
  gMCOConfig.heartbeat_msg.BUF[0] = 0;
  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;
  // init TPDOs
  while (i < NR_OF_TPDOS)
  {
    gTPDOConfig[i].CAN.ID = 0;
    i++;
  }
#endif
#if NR_OF_RPDOS > 0
  i = 0;
  // init RPDOs
  while (i < NR_OF_RPDOS)
  {
    gRPDOConfig[i].CANID = 0;
    i++;
  }
#endif

  // init the CAN interface
  if (!MCOHW_Init(Baudrate))
  {
    MCOUSER_FatalError(0x8802);
  }
  // for nmt master message
  if (!MCOHW_SetCANFilter(0))
  {
    MCOUSER_FatalError(0x8803);
  }
  // for SDO requests
  if (!MCOHW_SetCANFilter(0x600+Node_ID))
  {
    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 offset.
NOTE:    For data consistency, the application should not read the data
         while function MCO_ProcessStack executes.
RETURNS: nothing
**************************************************************************/
void MCO_InitRPDO
  (
  UNSIGNED8 PDO_NR,       // RPDO number (1-4)
  UNSIGNED16 CAN_ID,       // CAN identifier to be used (set to 0 to use default)
  UNSIGNED8 len,          // Number of data bytes in RPDO
  UNSIGNED8 offset        // Offset to data location in process image
  )
{

#ifdef CHECK_PARAMETERS
  // check PDO range and check node id range 1 - 127
  if (((PDO_NR < 1)             || (PDO_NR > NR_OF_RPDOS))      || 
      ((gMCOConfig.Node_ID < 1) || (gMCOConfig.Node_ID > 127)))
  {
    MCOUSER_FatalError(0x8804);
  }
  // is size of process image exceeded?
  if (offset >= PROCIMG_SIZE)   
  { 
    MCOUSER_FatalError(0x8904);
  }
#endif
  PDO_NR--;
  gRPDOConfig[PDO_NR].len = len;
  gRPDOConfig[PDO_NR].offset = offset;
  if (CAN_ID == 0)
  {
    gRPDOConfig[PDO_NR].CANID = 0x200 + (0x100 * ((UNSIGNED16)(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.
RETURNS: nothing
**************************************************************************/
void MCO_InitTPDO
  (
  UNSIGNED8 PDO_NR,        // TPDO number (1-4)
  UNSIGNED16 CAN_ID,        // CAN identifier to be used (set to 0 to use default)
  UNSIGNED16 event_time,    // Transmitted every event_tim ms 
  UNSIGNED16 inhibit_time,  // Inhibit time in ms for change-of-state transmit
                      // (set to 0 if ONLY event_tim should be used)
  UNSIGNED8 len,           // Number of data bytes in TPDO
  UNSIGNED8 offset         // Offset to data location in process image
  )
{

#ifdef CHECK_PARAMETERS
  // check PDO range, node id, len range 1 - 8 and event time or inhibit time set
  if (((PDO_NR < 1)             || (PDO_NR > NR_OF_TPDOS))     ||
      ((gMCOConfig.Node_ID < 1) || (gMCOConfig.Node_ID > 127)) ||
      ((len < 1)                || (len > 8))                  ||
      ((event_time == 0)        && (inhibit_time == 0)))
  {
    MCOUSER_FatalError(0x8806);
  }
  // is size of process image exceeded?
  if (offset >= PROCIMG_SIZE)   
  { 
    MCOUSER_FatalError(0x8906);
  }
#endif
  PDO_NR--;
  if (CAN_ID == 0)
  {
    gTPDOConfig[PDO_NR].CAN.ID = 0x180 + (0x100 * ((UNSIGNED16)(PDO_NR))) + gMCOConfig.Node_ID;
  }
  else
  {
    gTPDOConfig[PDO_NR].CAN.ID = CAN_ID;
  }
  gTPDOConfig[PDO_NR].CAN.LEN = len;
  gTPDOConfig[PDO_NR].offset = offset;
#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.
RETURNS: 0 if nothing was done, 1 if a CAN message was sent or received
**************************************************************************/
UNSIGNED8 MCO_ProcessStack
  (
  void
  )
{
  UNSIGNED8 i;
  UNSIGNED8 ret_val = 0;

  // check if this is right after boot-up
  // was set by MCO_Init
  if (gTPDONr == 0xFF)
  {
    // 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
    // going into operational state
    gMCOConfig.heartbeat_msg.BUF[0] = 0x05;
#if NR_OF_TPDOS > 0
    MCO_Prepare_TPDOs();
#endif
#else
    // going into pre-operational state
    gMCOConfig.heartbeat_msg.BUF[0] = 0x7F;
#endif
    // return value to default
    gTPDONr = NR_OF_TPDOS;
    return 1;
  }
 
  // work on next incoming messages
  // if message received
  if (MCOHW_PullMessage(&gRxCAN))
  {
    // is it an NMT master message?
    if (gRxCAN.ID == 0)
    {
      // nmt message is for this node or all nodes
      if ((gRxCAN.BUF[1] == gMCOConfig.Node_ID) || (gRxCAN.BUF[1] == 0))
      {
        switch (gRxCAN.BUF[0])
        {
          // start node
          case 1:
            gMCOConfig.heartbeat_msg.BUF[0] = 5;
#if NR_OF_TPDOS > 0          
            MCO_Prepare_TPDOs();
#endif
            break;

          // stop node
          case 2:
            gMCOConfig.heartbeat_msg.BUF[0] = 4;
            break;

          // enter pre-operational
          case 128:
            gMCOConfig.heartbeat_msg.BUF[0] = 127;
            break;
   
          // node reset
          case 129:
            MCOUSER_ResetApplication();
            break;

          // node reset communication
          case 130:
            MCOUSER_ResetCommunication();
            break;

          // unknown command
          default:
            break;
        }

        return 1;
      } // NMT message addressed to this node
    } // NMT master message received
    
    // if node is not stopped...
    if (gMCOConfig.heartbeat_msg.BUF[0] != 4)
    {
      // is the message an SDO request message for us?
      if (gRxCAN.ID == gMCOConfig.Node_ID+0x600)
      {
        // handle SDO request - return value not used in this version
        i = MCO_Handle_SDO_Request(&gRxCAN.BUF[0]);
        return 1;
      }
    }

#if NR_OF_RPDOS > 0
    // is the node operational?
    if (gMCOConfig.heartbeat_msg.BUF[0] == 5)
    {
      i = 0;
      // loop through RPDOs
      while (i < NR_OF_RPDOS)
      {
        // is this one of our RPDOs?
        if (gRxCAN.ID == gRPDOConfig[i].CANID)
        {
          // copy data from RPDO to process image
          memcpy(&(gProcImg[gRPDOConfig[i].offset]),&(gRxCAN.BUF[0]),gRPDOConfig[i].len);
          // exit the loop
          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
  // is the node operational?
  if (gMCOConfig.heartbeat_msg.BUF[0] == 5)
  {
    // check next TPDO for transmission
    gTPDONr++;
    if (gTPDONr >= NR_OF_TPDOS)
    {
      gTPDONr = 0;
    }
    // is the TPDO 'gTPDONr' in use?
    if (gTPDOConfig[gTPDONr].CAN.ID != 0)
    {
#ifdef USE_EVENT_TIME
      // does TPDO use event timer and event timer is expired? if so we need to transmit now
      if ((gTPDOConfig[gTPDONr].event_time != 0) && 
          (MCOHW_IsTimeExpired(gTPDOConfig[gTPDONr].event_timestamp)) )
      {
        // get data from process image and transmit
        memcpy(&(gTPDOConfig[gTPDONr].CAN.BUF[0]),&(gProcImg[gTPDOConfig[gTPDONr].offset]),gTPDOConfig[gTPDONr].CAN.LEN);
        MCO_TransmitPDO(gTPDONr);
        return 1;
      }
#endif // USE_EVENT_TIME
#ifdef USE_INHIBIT_TIME
      // does the TPDO use an inhibit time? - COS transmission
      if (gTPDOConfig[gTPDONr].inhibit_time != 0)
      {
        // is the inihibit timer currently running?
        if (gTPDOConfig[gTPDONr].inhibit_status > 0)
        {
          // has the inhibit time expired?
          if (MCOHW_IsTimeExpired(gTPDOConfig[gTPDONr].inhibit_timestamp))
          {
            // is there a new transmit message already waiting?
            if (gTPDOConfig[gTPDONr].inhibit_status == 2)
            { 
              // transmit now
              MCO_TransmitPDO(gTPDONr);
              return 1;
            }
            // no new message waiting, but timer expired
            else 
            {
              gTPDOConfig[gTPDONr].inhibit_status = 0;
            }
          }
        }
        // is inhibit status 0 or 1?
        if (gTPDOConfig[gTPDONr].inhibit_status < 2)
        {
          // has application data changed?
          if ((memcmp(&gTPDOConfig[gTPDONr].CAN.BUF[0],&(gProcImg[gTPDOConfig[gTPDONr].offset]),gTPDOConfig[gTPDONr].CAN.LEN) != 0))
          {
            // Copy application data
            memcpy(&gTPDOConfig[gTPDONr].CAN.BUF[0],&(gProcImg[gTPDOConfig[gTPDONr].offset]),gTPDOConfig[gTPDONr].CAN.LEN);
            // has inhibit time expired?
            if (gTPDOConfig[gTPDONr].inhibit_status == 0)
            {
              // transmit now
              MCO_TransmitPDO(gTPDONr);
              return 1;
            }
            // inhibit status is 1
            else
            {
              // wait for inhibit time to expire 
              gTPDOConfig[gTPDONr].inhibit_status = 2;
            }
          }
        }
      } // Inhibit Time != 0
#endif // USE_INHIBIT_TIME
    } // PDO active (CAN_ID != 0)  
  } // if node is operational
#endif // NR_OF_TPDOS > 0
  
  // do we produce a heartbeat?
  if (gMCOConfig.heartbeat_time != 0)
  {
    // has heartbeat time passed?
    if (MCOHW_IsTimeExpired(gMCOConfig.heartbeat_timestamp))
    {
      // transmit heartbeat message
      if (!MCOHW_PushMessage(&gMCOConfig.heartbeat_msg))
      {
        MCOUSER_FatalError(0x8801);
      }
      // get new heartbeat time for next transmission
      gMCOConfig.heartbeat_timestamp = MCOHW_GetTime() + gMCOConfig.heartbeat_time;
      ret_val = 1;
    }
  }
  return ret_val;
}