can.c

AVR平台下的CanOpen协议桟源码包括应用

#endif /* __VARS_IN_EEPROM__ */

  if( CANopenOpStateInit )
    state = NMT_OPERATIONAL;
  else
    state = NMT_PREOPERATIONAL;

  /* Update node state in NodeGuard message buffer
     (which may be sent automatically) */
  CAN_INT_DISABLE();
  can_write_reg( C91_MSGS_I + (C91_NODEGUARD*C91_MSG_SIZE),
		 state | (NodeGuardToggle & 0x80) );
  CAN_INT_ENABLE();

  return state;
}

/* ------------------------------------------------------------------------ */

BOOL can_set_opstate_init( BOOL enable )
{
  if( enable > 1 ) return FALSE;
  if( enable )
    CANopenOpStateInit = TRUE;
  else
    CANopenOpStateInit = FALSE;

#ifdef __VARS_IN_EEPROM__
  if( eeprom_read( EE_CANOPEN_OPSTATE_INIT ) != CANopenOpStateInit )
    eeprom_write( EE_CANOPEN_OPSTATE_INIT, CANopenOpStateInit );
#endif /* __VARS_IN_EEPROM__ */
  return TRUE;
}

/* ------------------------------------------------------------------------ */

BOOL can_get_opstate_init( void )
{
#ifdef __VARS_IN_EEPROM__
  CANopenOpStateInit = eeprom_read( EE_CANOPEN_OPSTATE_INIT );
#endif /* __VARS_IN_EEPROM__ */

  return CANopenOpStateInit;
}

/* ------------------------------------------------------------------------ */

BOOL can_set_busoff_maxcnt( BYTE cnt )
{
  CanBusOffMaxCnt = cnt;

#ifdef __VARS_IN_EEPROM__
  if( eeprom_read( EE_CAN_BUSOFF_MAXCNT ) != CanBusOffMaxCnt )
    eeprom_write( EE_CAN_BUSOFF_MAXCNT, CanBusOffMaxCnt );
#endif /* __VARS_IN_EEPROM__ */

  return TRUE;
}

/* ------------------------------------------------------------------------ */

BYTE can_get_busoff_maxcnt( void )
{
#ifdef __VARS_IN_EEPROM__
  CanBusOffMaxCnt = eeprom_read( EE_CAN_BUSOFF_MAXCNT );
#endif /* __VARS_IN_EEPROM__ */

  return CanBusOffMaxCnt;
}

/* ------------------------------------------------------------------------ */

/* Up to 16 bytes of configuration parameters can be stored */
#define CAN_STORE_SIZE 3

/* ------------------------------------------------------------------------ */

BOOL can_store_config( void )
{
  BYTE block[CAN_STORE_SIZE];

#ifdef __VARS_IN_EEPROM__
  RtrDisabled        = eeprom_read( EE_RTR_DISABLED );
  CANopenOpStateInit = eeprom_read( EE_CANOPEN_OPSTATE_INIT );
  CanBusOffMaxCnt    = eeprom_read( EE_CAN_BUSOFF_MAXCNT );
#endif /* __VARS_IN_EEPROM__ */

  block[0] = RtrDisabled;
  block[1] = CANopenOpStateInit;
  block[2] = CanBusOffMaxCnt;

  return( store_write_block( STORE_CAN, CAN_STORE_SIZE, block ) );
}

/* ------------------------------------------------------------------------ */

static void can_load_config( void )
{
  BYTE block[CAN_STORE_SIZE];

  /* Read the configuration from EEPROM, if any
     (errors in reading this datablock are caught and
      reported by functions in store.c...) */
  if( store_read_block( STORE_CAN, CAN_STORE_SIZE, block ) )
    {
      RtrDisabled        = block[0];
      CANopenOpStateInit = block[1];
      CanBusOffMaxCnt    = block[2];
    }
  else
    {
      /* No valid parameters in EEPROM: use defaults */
      RtrDisabled        = FALSE;
      CANopenOpStateInit = FALSE;
      CanBusOffMaxCnt    = 5;
    }

#ifdef __VARS_IN_EEPROM__
  /* Create working copies of configuration globals in EEPROM */
  if( eeprom_read( EE_RTR_DISABLED ) != RtrDisabled )
    eeprom_write( EE_RTR_DISABLED, RtrDisabled );
  if( eeprom_read( EE_CANOPEN_OPSTATE_INIT ) != CANopenOpStateInit )
    eeprom_write( EE_CANOPEN_OPSTATE_INIT, CANopenOpStateInit );
  if( eeprom_read( EE_CAN_BUSOFF_MAXCNT ) != CanBusOffMaxCnt )
    eeprom_write( EE_CAN_BUSOFF_MAXCNT, CanBusOffMaxCnt );
#endif /* __VARS_IN_EEPROM__ */
}

/* ------------------------------------------------------------------------ */
/* CAN INT interrupt handler */

#pragma interrupt_handler canint_handler:3

void canint_handler( void )
{
  BYTE object_no;

  object_no = can_check_for_msgs();

  if( object_no != NO_OBJECT )
    {
      /* CAN message received: copy it to the CAN message buffer */
      BYTE        cntr;
      BYTE        index;
      BYTE        *msg;
      BYTE        dlc;
      signed char byt;

      cntr = get_buf_cntr();

      /* If buffer is full (keep one buffer 'free', it might be the one
	 currently being processed by the application, and since the data
	 bytes are not copied they should not be overwritten yet; also the
	 buffer space is used for assembling a reply, by the SDO server),
	 disable further interrupts to prevent overwriting message buffers */
      if( cntr == CAN_BUFS-1 )
	{
	  CAN_INT_DISABLE();

	  /* A message is lost: get it reported; also because the interrupt
	     has been disabled more messages might get lost! */
	  CanBufFull = TRUE;

	  return;
	}

      /* Calculate index (of first empty buffer) from the 'done-reading-until'
	 index (MsgOutIndex_) and the 'number-of-full-buffers' counter
	 (MsgCounter_) parameters, which are stored in a fault-tolerant way
	 (3 copies of each parameter, majority voting mechanism) */
      index = (get_buf_index() + cntr) & CAN_BUFS_MASK;

      /* Location to copy CAN message to */
      msg = CanMsgBuf[index];

      /* Get received DLC */
      if( object_no > C91_MSG_BUFFERS-1 )
	{
	  dlc = 0; /* These object_no values are reserved for RTRs */
	}
      else
	{
	  BYTE addr;

	  dlc = (can_read_reg( C91_DR00_I+1+(object_no<<1) ) &
		 C91_DR_DLC_MASK);

	  /* Read the data bytes from the message buffer;
	     NB: always read the 8th databyte to guarantee a reload of
	     the message buffer to the Shadow Register !!!
	     If 2 messages with the same COB-ID arrive one after the other and
	     have less than 8 bytes, then the second 'read' would otherwise
	     *NOT* result in the new databytes !!!) */

	  /* Determine object's message buffer address in CAN-controller */
	  addr = C91_MSGS_I + (object_no * C91_MSG_SIZE);

	  /* Force transfer to Shadow Register */
	  if( dlc < 8 )
	    can_read_reg( addr + 7 );
	  else
	    dlc = 8;

	  /* Copy data bytes */
	  for( byt=dlc-1; byt>=0; --byt ) msg[byt] = can_read_reg( addr+byt );
	}

      /* Store Object ID, DLC and mark buffer as 'not empty' */
      msg[MSG_OBJECT_I] = object_no;
      msg[MSG_DLC_I]    = dlc;
      msg[MSG_VALID_I]  = BUF_NOT_EMPTY;

      /* Increment the CAN-message-in-buffer counter */
      ++cntr;
      MsgCounter1 = cntr;  MsgCounter2 = cntr;  MsgCounter3 = cntr;
    }
}

/* ------------------------------------------------------------------------ */

static BYTE can_check_for_msgs( void )
{
  BYTE object_no, bitmask;

  /* Each time 'can_check_for_msgs()') is called
     'ObjectMask1' and 'ObjectMask2' are updated */

  /* Check Receive-Ready register 1 */
  ObjectMask1 |= can_read_reg( C91_RECV_READY1_I );

  /* There is at least one message in buffers 0-7 received
     and/or still to be handled */
  if( ObjectMask1 != 0 )
    {
      bitmask = 0x01;
      for( object_no=0; object_no<C91_MSG_BUFFERS_PER_RRR; ++object_no )
	{
	  if( ObjectMask1 & bitmask )
	    {
	      /* Clear the bit in the Receive Ready register */
	      can_write_reg( C91_RECV_READY1_I, ~bitmask );

	      /* Reset the RI interrupt bit (if possible) */
	      // ###Not necessary ?
	      //can_write_reg( C91_INTERRUPT_I, (BYTE)(~C91_RECV_INT) );

	      /* Clear the bit in 'ObjectMask1' */
	      ObjectMask1 &= ~bitmask;

	      /* Remote Transmission Requests require extra work... */
	      if( object_no == C91_RTR )
		{
		  BYTE id_lo, id_hi;

		  /* Check if RTR received is for this node */

		  id_lo = can_read_reg( C91_DR00_I+(2*C91_RTR)+1 );

#ifdef __VARS_IN_EEPROM__
		  /* ### Not in interrupt routine */
		  //RtrIdLo = eeprom_read( EE_RTRIDLO );
#endif
		  if( (id_lo &
		       (NODEID_MASK_LOW_BYTE | C91_DR_RTR_MASK)) == RtrIdLo )
		    {
		      id_hi = can_read_reg( C91_DR00_I+(2*C91_RTR) );

#ifdef __VARS_IN_EEPROM__
		      /* ### Not in interrupt routine */
		      //RtrIdHi = eeprom_read( EE_RTRIDHI );
#endif
		      if( (id_hi & NODEID_MASK_HIGH_BYTE) == RtrIdHi )
			{
			  /* Okay, this is an RTR for me;
			     find out which object is requested... */

			  id_hi &= OBJECT_MASK;

			  switch( id_hi )
			    {
			    case TPDO1_OBJ:
			      return C91_TPDO1_RTR;
			    case TPDO2_OBJ:
			      return C91_TPDO2_RTR;
			    case TPDO3_OBJ:
			      return C91_TPDO3_RTR;
			    case NODEGUARD_OBJ:
			      return C91_NODEGUARD_RTR;
			    default:
			      /* Don't service this message */
			      return NO_OBJECT;
			    }
			}
		      else
			{
			  /* Don't service this message */
			  return NO_OBJECT;
			}
		    }
		  else
		    {
		      /* Don't service this message */
		      return NO_OBJECT;
		    }
		}
	      else
		{
		  /* This corresponds to the message buffer number
		     containing a message to service */
		  return object_no;
		}
	    }
	  bitmask <<= 1;
	}
    }

  /* Check Receive-Ready register 2 */
  ObjectMask2 |= can_read_reg( C91_RECV_READY2_I );

  /* There is at least one message in buffers 8-15 received
     and/or still to be handled */
  if( ObjectMask2 != 0 )
    {
      bitmask = 0x01;
      for( object_no=0; object_no<C91_MSG_BUFFERS_PER_RRR; ++object_no )
	{
	  if( ObjectMask2 & bitmask )
	    {
	      /* Clear the bit in the Receive Ready register */
	      can_write_reg( C91_RECV_READY2_I, ~bitmask );

	      /* Reset the RI interrupt bit (if possible) */
	      // ###Not necessary ?
	      //can_write_reg( C91_INTERRUPT_I, (BYTE)(~C91_RECV_INT) );

	      /* Clear the bit in 'ObjectMask2' */
	      ObjectMask2 &= ~bitmask;

	      /* This corresponds to the message buffer number
		 containing a message to service */
	      return( C91_MSG_BUFFERS_PER_RRR + object_no );
	    }
	  bitmask <<= 1;
	}
    }

  /* No message to service */
  return NO_OBJECT;
}

/* ------------------------------------------------------------------------ */

static BYTE get_buf_index( void )
{
  /* Majority voting */
  if( MsgOutIndex1 == MsgOutIndex2 ) MsgOutIndex3 = MsgOutIndex1;
  else if( MsgOutIndex1 == MsgOutIndex3 ) MsgOutIndex2 = MsgOutIndex1;
  else if( MsgOutIndex2 == MsgOutIndex3 ) MsgOutIndex1 = MsgOutIndex3;
  else
    {
      /* All 3 are different: do a majority vote on a bit-by-bit basis... */
      BYTE byt, bitmask, bits, i;
      byt     = 0x00; /* Start value */
      bitmask = 0x80; /* Start with MSB */
      for( i=0; i<8; ++i )
	{
	  bits = 0;
	  if( MsgOutIndex1 & bitmask ) ++bits;
	  if( MsgOutIndex2 & bitmask ) ++bits;
	  if( MsgOutIndex3 & bitmask ) ++bits;
	  byt <<= 1; /* Shift value one bit up*/
	  /* Bit is set or not */
	  if( bits > 1 ) ++byt; /* 2 or 3 of the bits (majority) are set */
	  bitmask >>= 1; /* Next bit to check */
	}
      /* Set the new MsgOutIndex1/2/3 value */
      MsgOutIndex1 = byt;  MsgOutIndex2 = byt;  MsgOutIndex3 = byt;
    }
  return MsgOutIndex1;
}

/* ------------------------------------------------------------------------ */

static BYTE get_buf_cntr( void )
{
  /* Majority voting */
  if( MsgCounter1 == MsgCounter2 ) MsgCounter3 = MsgCounter1;
  else if( MsgCounter1 == MsgCounter3 ) MsgCounter2 = MsgCounter1;
  else if( MsgCounter2 == MsgCounter3 ) MsgCounter1 = MsgCounter3;
  else
    {
      /* All 3 are different: do a majority vote on a bit-by-bit basis... */
      BYTE byt, bitmask, bits, i;
      byt     = 0x00; /* Start value */
      bitmask = 0x80; /* Start with MSB */
      for( i=0; i<8; ++i )
	{
	  bits = 0;
	  if( MsgCounter1 & bitmask ) ++bits;
	  if( MsgCounter2 & bitmask ) ++bits;
	  if( MsgCounter3 & bitmask ) ++bits;
	  byt <<= 1; /* Shift value one bit up*/
	  if( bits > 1 ) ++byt; /* 2 or 3 of the bits (majority) are set */
	  bitmask >>= 1; /* Next bit to check */
	}
      /* Set the new MsgCounter1/2/3 value */
      MsgCounter1 = byt;  MsgCounter2 = byt;  MsgCounter3 = byt;
    }
  return MsgCounter1;
}

/* ------------------------------------------------------------------------ */

static BYTE bits_in_byte( BYTE val )
{
  /* Calculate the number of bits set in a byte
     (method taken from www.devx.com, 'C++ tips'):
     Add up 4 pairs of 1-bit numbers (resulting in four 2-bit numbers):
     val = ((val >> 1) & 0x55) + (val & 0x55);
     Optimisation: save an AND (&) instruction by exploiting this clever
     relationship for each pair of bits: the two bits 'ab' represent the
     number 2a+b; to count the bits we subtract 'a' (i.e. 2a+b - a = a+b) */

  val = val - ((val >> 1) & 0x55);

  /* Add up four 2-bit numbers (resulting in two 4-bit numbers */
  val = ((val >> 2) & 0x33) + (val & 0x33);

  /* Add up two 4-bit numbers resulting in one 8-bit number */
  val = ((val >> 4) & 0x0F) + (val & 0x0F);

  return val;
}

/* ------------------------------------------------------------------------ */