node.cpp

美国COPLEY驱动器,程序开发工具之一.

}

/***************************************************************************/
/**
Get the CANopen identity object for this node (object dictionary entry 0x1018).
Note that only the VendorID field is mandatory.  Any unsupported fields will
be returned as zero.

@param id The identity object to be filled in by this call
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::GetIdentity( NodeIdentity &id )
{
   // Find the number of supported fields
   int8 ct;
   const Error *err = sdo.Upld8( 0x1018, 0, ct );
   if( err ) return err;

   if( ct < 1 || ct > 4 )
      return &CanOpenError::IllegalFieldCt;

   id.productCode = 0;
   id.revision    = 0;
   id.serial      = 0;

   err = sdo.Upld32( 0x1018, 1, id.vendorID );
   if( !err && ct > 1 ) err = sdo.Upld32( 0x1018, 2, id.productCode );
   if( !err && ct > 2 ) err = sdo.Upld32( 0x1018, 3, id.revision    );
   if( !err && ct > 3 ) err = sdo.Upld32( 0x1018, 4, id.serial      );

   return err;
}

/***************************************************************************/
/**
Reset this node.
@return An error object
*/
/***************************************************************************/
const Error *Node::ResetNode( void )
{
   cml.Debug( "ResetNode %d.\n", GetNodeID() );

   // Disable the node guarding task
   mutex.Lock();
   guardType    = GUARDTYPE_NONE;
   guardTimeout = -1;
   guardToggle  = -1;
   guardEvents.setBits( GUARD_EVENT_CHANGE );
   mutex.Unlock();

   // Wait for the node guarding task to change it's state
   EventNone e = GUARD_EVENT_CHANGE;

   const Error *err = e.Wait( guardEvents, 500 );
   if( err )
   {
      cml.Warn( "Error %s waiting for node guard task during ResetNode call\n", err->toString() );
      return err;
   }

   // Send the node a reset message
   err = co->ResetNode( nodeID );
   if( err ) return err;

   // Wait for the state to change to pre-op without sending
   // a remote request.  The node should indicate that state
   // on boot up.
   return WaitStateChange( NODESTATE_PRE_OP, false );
}

/***************************************************************************/
/**
Disable node guarding & heartbeat monitoring.

@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::StopGuarding( void )
{
   // Clear both heartbeat and node guard times.
   sdo.Dnld16( 0x1017, 0, (uint16)0 );
   sdo.Dnld16( 0x100C, 0, (uint16)0 );

   mutex.Lock();
   guardType    = GUARDTYPE_NONE;
   guardTimeout = -1;
   guardToggle  = -1;
   guardEvents.setBits( GUARD_EVENT_CHANGE );
   mutex.Unlock();

   return 0;
}

/***************************************************************************/
/**
Enable heartbeat messages from this node, and start a thread to monitor
them.
@param period The producer timeout value (milliseconds).  The node will
       be configured to produce a heartbeat message at this interval.
		 
@param timeout The additional number of milliseconds that the monitor
       thread will wait before indicating an error.  Thus, the consumer
		 heartbeat interval will be (period + timeout).
		 
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::StartHeartbeat( uint16 period, uint16 timeout )
{
   // If the period is sent as zero, return an error.
   if( !period )
      return &CanOpenError::BadParam;

   // Set the node's heartbeat time.
   const Error *err = sdo.Dnld16( 0x1017, 0, period );

   if( !err )
   {
      mutex.Lock();
      guardType    = GUARDTYPE_HEARTBEAT;
      guardTimeout = (int32)period + (int32)timeout;
      guardToggle  = -1;
      guardEvents.setBits( GUARD_EVENT_CHANGE );
      mutex.Unlock();
   }

   return err;
}

/***************************************************************************/
/**
Enable node guarding on this node.

When node guarding is enabled, a new thread is created which will send a 
remote request to this node every (guardTime) milliseconds.  The node must
respond to this message within the guard time.  If the node does not respond
then the thread will notify the node of a state change.

@param guardTime The period in milliseconds of the guard messages sent
       to the node.  It can range from 1 to 65535.
		 
@param lifeFactor A multiplier used by the node to determine how long to 
       wait for a node guarding message from the host before indicating
		 a local error.  The nodes timeout (life time) is guardTime * lifeFactor.
		 This parameter must be between 0 and 255.  If it's zero, then life
		 guarding on the node is disabled.
		 
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::StartNodeGuard( uint16 guardTime, byte lifeFactor )
{
   // If guard time is zero, return an error.
   // Note that the lifeFactor can be zero, this disables the life
   // guarding inside the node.
   if( !guardTime )
      return &CanOpenError::BadParam;

   // Program the life time factor
   const Error *err;
   err = sdo.Dnld8( 0x100D, 0, lifeFactor );

   // Program the guard time
   if( !err ) err = sdo.Dnld16( 0x100C, 0, guardTime );

   // Try to set the heartbeat time to zero.
   // This may fail (since the heartbeat may not be
   // supported).
   if( !err ) sdo.Dnld16( 0x1017, 0, (uint16)0 );

   if( !err )
   {
      mutex.Lock();
      guardType    = GUARDTYPE_NODEGUARD;
      guardTimeout = guardTime;
      guardToggle  = -1;
      guardEvents.setBits( GUARD_EVENT_CHANGE );
      mutex.Unlock();
   }

   return err;
}

/***************************************************************************/
/**
Wait for the node state to change.  This function is used after sending out
a network management message intended to change the node's state.  It sends
a remote transmit request to the node and waits for the guard message to
be returned.

This process allows us to track the node's state locally without fear of 
race conditions that would arise if we simply assumed that sending out the
NMT message instantly caused the state to change to the new value.

@param newState The state that we expect the node to change to.
@param request If true (default), send a CAN message requesting state info.
               If false no request is sent.
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::WaitStateChange( NodeState newState, bool request )
{
   CanFrame frame;
   frame.id = nodeID + 0x700;
   frame.type = CAN_FRAME_REMOTE;
   frame.length = 1;

   EventAny e = GUARD_EVENT_DSRDSTATE;

   guardEvents.clrBits( GUARD_EVENT_DSRDSTATE );

   desired = newState;

   if( stateChangeDelay )
      sleep( stateChangeDelay );

   if( request ) co->Xmit( frame );

   return e.Wait( guardEvents, sdo.GetTimeout() );
}

/***************************************************************************/
/**
This method is called when a new node guarding / heartbeat message is received
from the node.
*/
/***************************************************************************/
int Node::NewFrame( CanFrame &frame )
{
   if( frame.type != CAN_FRAME_DATA )
      return 1;

   mutex.Lock();

   // Save the old node state
   NodeState oldState = state;

   // Check for a valid toggle bit.  The toggle bit is only
   // used when running in node guarding mode. 
   int toggleErr = guardToggle ^ (frame.data[0] & 0x80);

   // If the guardToggle variable is -1, then ignore toggle
   // errors.  This is used in the modes where the toggle bit
   // is not defined (heartbeat and no guarding).  It's also
   // used for the first node guarding message since I can't be
   // sure what the node's state is when I start and therefore
   // don't know if the toggle bit should be set.
   if( guardToggle < 0 )
      toggleErr = 0;

   if( !toggleErr )
   {
      switch( frame.data[0] & 0x7f )
      {
	 case 0:   state = NODESTATE_PRE_OP;      break;
	 case 4:   state = NODESTATE_STOPPED;     break;
	 case 5:   state = NODESTATE_OPERATIONAL; break;
	 case 127: state = NODESTATE_PRE_OP;      break;
	 default:  state = NODESTATE_UNKNOWN;     break;
      }

      uint32 bits = GUARD_EVENT_MSG_RVCD;

      // If a thread is waiting for this state, wake it.
      if( state == desired )
      {
	 bits |= GUARD_EVENT_DSRDSTATE;
	 desired = NODESTATE_INVALID;
      }

      // Notify the guarding task that a message was received.
      guardEvents.setBits( bits );
   }
   else
      cml.Warn( "Node %d guard toggle error\n", GetNodeID() );

   // If I'm using the node guarding protocol, update my
   // expected toggle bit value
   if( guardType == GUARDTYPE_NODEGUARD )
      guardToggle = (~frame.data[0]) & 0x80;

   mutex.Unlock();

   // Signal a state change
   if( state != oldState )
      HandleStateChange( oldState, state );

   return 1;
}

/***************************************************************************/
/**
Node object watchdog thread.  This thread monitors the node guarding and 
heartbeat protocols.  If the node stops responding within the required
timeouts, the thread will change the local state variable to indicate the
error.
*/
/***************************************************************************/
void Node::run( void )
{
   CanFrame frame;

   // Initialize my node guarding frame
   frame.id = nodeID + 0x700;
   frame.type = CAN_FRAME_REMOTE;
   frame.length = 1;

   while( 1 )
   {
      // Grab a local copy of the guard parameters safely
      int32 timeout;
      GuardProtocol gtype;

      mutex.Lock();
      timeout = guardTimeout;
      gtype = guardType;
      mutex.Unlock();

      // Clear the message received event bit
      guardEvents.clrBits( GUARD_EVENT_MSG_RVCD );

      /**************************************************
       * For the node guarding protocol; send a remote 
       * transmit request and sleep for the timeout 
       * period.  After sleeping, I see if the message
       * was received.
       **************************************************/
      if( gtype == GUARDTYPE_NODEGUARD )
      {
	 co->Xmit( frame );

	 EventAny e = GUARD_EVENT_CHANGE;

	 if( e.Wait( guardEvents, timeout ) == 0 )
	 {
	    cml.Debug( "Guard %d, event changed\n", nodeID );
	    guardEvents.clrBits( GUARD_EVENT_CHANGE );
	    continue;
	 }

	 // Now, use a zero timeout when getting the
	 // guard semaphore
	 timeout = 0;
      }

      /**************************************************
       * For the heartbeat protocol, just wait for a new
       * message.  This also works when all guarding is
       * disabled since the timeout should be -1 (forever)
       **************************************************/
      EventAny e = GUARD_EVENT_CHANGE | GUARD_EVENT_MSG_RVCD;
      const Error *err = e.Wait( guardEvents, timeout );

      // Handle timeouts
      if( err == &ThreadError::Timeout )
      {
	 cml.Debug( "Guard %d, timeout waiting on response\n", nodeID );
	 mutex.Lock();
	 NodeState oldState = state;
	 state = NODESTATE_GUARDERR;
	 mutex.Unlock();

	 HandleStateChange( oldState, NODESTATE_GUARDERR );
      }

      else if( err )
	 cml.Error( "Guard %d error: %s\n", nodeID, err->toString() );

      else if( e.getMask() & GUARD_EVENT_CHANGE )
      {
	 cml.Debug( "Guard %d, event change\n", nodeID );
	 guardEvents.clrBits( GUARD_EVENT_CHANGE );
      }
   }
}