mco.c

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

/**************************************************************************
MODULE:    MCO
CONTAINS:  MicroCANopen implementation
COPYRIGHT: Embedded Systems Academy, Inc. 2002-2005.
           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:   THIS IS THE EDUCATIONAL VERSION OF MICROCANOPEN
           See file license_educational.txt or
           www.microcanopen.com/license_educational.txt
           A commercial MicroCANopen license is available at
           www.CANopenStore.com
VERSION:   2.10, ESA 12-JAN-05
           $LastChangedDate: 2005-01-12 13:53:59 -0700 (Wed, 12 Jan 2005) $
           $LastChangedRevision: 48 $
***************************************************************************/ 

#include "mco.h"
#include "mcohw.h"
#include <string.h>

/**************************************************************************
GLOBAL VARIABLES
***************************************************************************/ 

// this structure holds all node specific configuration
MCO_CONFIG MEM_FAR gMCOConfig;

#if NR_OF_TPDOS > 0
// this structure holds all the TPDO configuration data for up to 4 TPDOs
TPDO_CONFIG MEM_FAR gTPDOConfig[NR_OF_TPDOS];
#endif

// this is the next TPDO to be checked in MCO_ProcessStack
UNSIGNED8 MEM_FAR gTPDONr = NR_OF_TPDOS;

#if NR_OF_RPDOS > 0
// this structure holds all the RPDO configuration data for up to 4 RPDOs
RPDO_CONFIG MEM_FAR gRPDOConfig[NR_OF_RPDOS];
#endif

// this structure holds the current receive message
CAN_MSG MEM_FAR gRxCAN;

// this structure holds the CAN message for SDO responses or aborts
CAN_MSG MEM_FAR gTxSDO;

// process image from user_xxxx.c
extern UNSIGNED8 MEM_NEAR gProcImg[];

// table with SDO Responses for read requests to OD - defined in user_xxx.c
extern UNSIGNED8 MEM_CONST SDOResponseTable[];


/**************************************************************************
LOCAL FUNCTIONS
***************************************************************************/

// SDO Abort Messages
#define SDO_ABORT_UNSUPPORTED     0x06010000UL
#define SDO_ABORT_NOT_EXISTS      0x06020000UL
#define SDO_ABORT_READONLY        0x06010002UL
#define SDO_ABORT_TYPEMISMATCH    0x06070010UL
#define SDO_ABORT_UNKNOWN_COMMAND 0x05040001UL
#define SDO_ABORT_UNKNOWNSUB      0x06090011UL

/**************************************************************************
DOES:    Search the SDO Response table for a specifc index and subindex.
RETURNS: 255 if not found, otherwise the number of the record found
         (staring at zero)
**************************************************************************/
UNSIGNED8 MCO_Search_OD
  (
  UNSIGNED16 index,   // Index of OD entry searched
  UNSIGNED8 subindex // Subindex of OD entry searched 
  )
{
  UNSIGNED8 i;
  UNSIGNED8 i_hi, hi;
  UNSIGNED8 i_lo, lo;
  UNSIGNED8 const *p;
  UNSIGNED8 const *r;

  i = 0;
  i_hi = (UNSIGNED8) (index >> 8);
  i_lo = (UNSIGNED8) index;
  r = &(SDOResponseTable[0]);
  while (i < 255)
  {
    p = r;
    // set r to next record in table
    r += 8;
    // skip command byte
    p++;
    lo = *p;
    p++;
    hi = *p;
    // if index in table is 0xFFFF, then this is the end of the table
    if ((lo == 0xFF) && (hi == 0xFF))
    {
      return 255;
    }
    if (lo == i_lo)
    { 
      if (hi == i_hi)
      { 
        p++;
        // entry found?
        if (*p == subindex)
        {
          return i;
        }
      }
    }
    i++;
  }

  // not found
  return 255;
}


/**************************************************************************
DOES:    Generates an SDO Abort Response
RETURNS: nothing
**************************************************************************/
void MCO_Send_SDO_Abort
  (
  UNSIGNED32 ErrorCode  // 4 byte SDO abort error code
  )
{
  UNSIGNED8 i;

  // construct message data
  gTxSDO.BUF[0] = 0x80;
  for (i=0;i<4;i++)
  {
    gTxSDO.BUF[4+i] = ErrorCode;
    ErrorCode >>= 8;
  }

  // transmit message
  if (!MCOHW_PushMessage(&gTxSDO))
  {
    // failed to transmit
    MCOUSER_FatalError(0x8801);
  }
}

/**************************************************************************
DOES:    Handle an incoimg SDO request.
RETURNS: returns 1 if SDO access success, returns 0 if SDO abort generated
**************************************************************************/
UNSIGNED8 MCO_Handle_SDO_Request 
  (
  UNSIGNED8 *pData  // pointer to 8 data bytes with SDO data
  )
{
  // command byte of SDO request
  UNSIGNED8 cmd;
  // index of SDO request
  UNSIGNED16 index;
  // subindex of SDO request
  UNSIGNED8 subindex;
  // search result of Search_OD
  UNSIGNED8 found;

  // init variables
  // upper 3 bits are the command
  cmd = *pData & 0xE0;
  // get high byte of index
  index = pData[2];
  // add low byte of index
  index = (index << 8) + pData[1];
  // subindex
  subindex = pData[3];

  // Copy Multiplexor into response
  // index low
  gTxSDO.BUF[1] = pData[1];
  // index high
  gTxSDO.BUF[2] = pData[2];
  // subindex
  gTxSDO.BUF[3] = pData[3];

  // is it a read or write command?
  if ((cmd == 0x40) || (cmd == 0x20)) 
  {

    // search const table 
    found = MCO_Search_OD(index,subindex);
    // entry found?
    if (found < 255)
    {
      // read command?
      if (cmd == 0x40)
      {
        memcpy(&gTxSDO.BUF[0],&SDOResponseTable[(found*8)],8);
        if (!MCOHW_PushMessage(&gTxSDO))
        {
          MCOUSER_FatalError(0x8802);
        }
        return 1;
      }
      // write command
      MCO_Send_SDO_Abort(SDO_ABORT_READONLY);
      return 0;
    }
    if ((index == 0x1001) && (subindex == 0x00))
    {
      // read command
      if (cmd == 0x40)
      {
        // expedited, 1 byte of data
        gTxSDO.BUF[0] = 0x4F;
        gTxSDO.BUF[4] = gMCOConfig.error_register;
        if (!MCOHW_PushMessage(&gTxSDO))
        {
          MCOUSER_FatalError(0x8802);
        }
        return 1;
      }
      // write command
      MCO_Send_SDO_Abort(SDO_ABORT_READONLY);
      return 0;
    }

#ifdef DYNAMIC_HEARTBEAT
    // hard coding of dynamic read/write accesses
    // access to [1017,00] - heartbeat time
    if ((index == 0x1017) && (subindex == 0x00))
    {
      // read command
      if (cmd == 0x40)
      {
        // expedited, 2 bytes of data
        gTxSDO.BUF[0] = 0x4B;
        gTxSDO.BUF[4] = (UNSIGNED8) gMCOConfig.heartbeat_time;
        gTxSDO.BUF[5] = (UNSIGNED8) (gMCOConfig.heartbeat_time >> 8);
        if (!MCOHW_PushMessage(&gTxSDO))
        {
          MCOUSER_FatalError(0x8802);
        }
        return 1;
      }
      // expedited write command with 2 bytes of data
      if (*pData == 0x2B)
      {
        gMCOConfig.heartbeat_time = pData[5];
        gMCOConfig.heartbeat_time = (gMCOConfig.heartbeat_time << 8) + pData[4];
        // write response
        gTxSDO.BUF[0] = 0x60;
        // Needed to pass conformance test: clear unused bytes
        gTxSDO.BUF[4] = 0;
        gTxSDO.BUF[5] = 0;
        gTxSDO.BUF[6] = 0;
        gTxSDO.BUF[7] = 0;
        if (!MCOHW_PushMessage(&gTxSDO))
        {
          MCOUSER_FatalError(0x8802);
        }
        return 1;
      }
      MCO_Send_SDO_Abort(SDO_ABORT_UNSUPPORTED);
      return 0;
    }
#endif // DYNAMIC HEARTBEAT

    // Requested OD entry not found
    if (subindex == 0)
    {
      MCO_Send_SDO_Abort(SDO_ABORT_NOT_EXISTS);
    }
    else
    {
      MCO_Send_SDO_Abort(SDO_ABORT_UNKNOWNSUB);
    }
    return 0;
  }
  // ignore abort received - all other produce an error
  if (cmd != 0x80)
  {
    MCO_Send_SDO_Abort(SDO_ABORT_UNKNOWN_COMMAND);
    return 0;
  }
  return 1;
}


#if NR_OF_TPDOS > 0
/**************************************************************************
DOES:    Called when going into the operational mode.
         Prepares all TPDOs for operational.
RETURNS: nothing
**************************************************************************/
void MCO_Prepare_TPDOs 
  (
    void
  )
{
UNSIGNED8 i;

  i = 0;
  // prepare all TPDOs for transmission
  while (i < NR_OF_TPDOS)
  {
    // this TPDO is used
    if (gTPDOConfig[i].CAN.ID != 0)
    {
      // Copy current process data
      memcpy(&gTPDOConfig[i].CAN.BUF[0],&(gProcImg[gTPDOConfig[i].offset]),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++;
  }
  // ensure that MCO_ProcessStack starts with TPDO1
  gTPDONr = NR_OF_TPDOS;
}

/**************************************************************************
DOES:    Called when a TPDO needs to be transmitted
RETURNS: nothing
**************************************************************************/
void MCO_TransmitPDO 
  (
  UNSIGNED8 PDONr  // TPDO number to transmit
  )
{
#ifdef USE_INHIBIT_TIME
  // new inhibit timer started
  gTPDOConfig[PDONr].inhibit_status = 1;
  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
  if (!MCOHW_PushMessage(&gTPDOConfig[PDONr].CAN))
  {
    MCOUSER_FatalError(0x8801);
  }
}
#endif // NR_OF_TPDOS > 0

/**************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/ 

/**************************************************************************
DOES:    Initializes the MicroCANopen stack
         It must be called from within MCOUSER_ResetApplication
RETURNS: nothing
**************************************************************************/
void MCO_Init 
  (
  UNSIGNED16 Baudrate,  // CAN baudrate in kbit (1000,800,500,250,125,50,25 or 10)
  UNSIGNED8 Node_ID,   // CANopen node ID (1-126)
  UNSIGNED16 Heartbeat  // Heartbeat time in ms (0 for none)
  )
{
  UNSIGNED8 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;