node.cpp

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

/************************************************************/
/*                                                          */
/*  Copley Motion Libraries                                 */
/*                                                          */
/*  Author: Stephen Glow                                    */
/*                                                          */
/*  Copyright (c) 2002-2005 Copley Controls Corp.           */
/*                          http://www.copleycontrols.com   */
/*                                                          */
/************************************************************/

/** \file
This file holds code to implement the CANopen node related objects.
*/

#include "CML.h"

CML_NAMESPACE_USE();

/**************************************************
* Node Error objects
**************************************************/
CML_NEW_ERROR( NodeError, GuardTimeout, "Node guarding timeout" );

/**************************************************
* Bits used with the node guarding event map
**************************************************/
#define GUARD_EVENT_MSG_RVCD     0x00000001
#define GUARD_EVENT_CHANGE       0x00000002
#define GUARD_EVENT_DSRDSTATE    0x00000004

/***************************************************************************/
/**
Initialize a CANopen node emergency receiver.  This function should be called
once and only once for each emergency receiver.
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *NodeEmcyRcvr::Init( Node *nodePtr )
{
   // Return an error if the object has already been initialized.
   if( IsInitialized() ) return &CanOpenError::Initialized;

   // Init the object
   node = nodePtr;

   return Receiver::Init( *(node->co), 0x80 + node->nodeID );
}

/***************************************************************************/
/**
Handle an node's emergency object.  The HandleEmergency() member function 
of the node that this emergency handler is associated with is called.
@param frame The emergency frame
@return Always returns 1.
*/
/***************************************************************************/
int NodeEmcyRcvr::NewFrame( CanFrame &frame )
{
   node->HandleEmergency( frame );
   return 1;
}

/***************************************************************************/
/**
Default CANopen node object constructor.  This constructor simple marks the
object as uninitialized.  The Init() function must be called before this
object can be used.
*/
/***************************************************************************/
Node::Node()
{
   stateChangeDelay = 0;
}

/***************************************************************************/
/**
Initialize the CANopen Node object.
@param canOpen The CANopen network object that this node is associated with.
@param nodeID The node's ID.  This must range from 1 to 127 for the node to
              be successfully initialized.
*/
/***************************************************************************/
Node::Node( CanOpen &canOpen, int16 nodeID )
{
   Init( canOpen, nodeID );
}

/***************************************************************************/
/**
CANopen node destructor.
*/
/***************************************************************************/
Node::~Node()
{
   cml.Debug( "Node: %d destroyed\n", nodeID );
   UnInit();
}

/***************************************************************************/
/**
Initialize the CANopen Node object.  Note that a CANopen node object must
be initialized once and only once.  This function should be used to initialize
the object if it was created using the default constructor.

@param canOpen The CANopen network object that this node is associated with.
@param nodeID The node's ID.  This must range from 1 to 127 for the node to
              be successfully initialized.
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::Init( CanOpen &canOpen, int16 nodeID )
{
   const Error *err;

   // Check for invalid node ID
   if( nodeID < 1 || nodeID > 127 )
      return &CanOpenError::BadNodeID;

   // Un-initialize first if necessary
   if( IsInitialized() && (err = UnInit()) != 0 )
      return err;

   // Init some local parameters
   this->nodeID = nodeID;
   state = NODESTATE_UNKNOWN;

   // Start out with node guarding disabled, and 
   // start the guard monitor thread.
   guardEvents.clrBits( 0xFFFFFFFF );
   guardTimeout = -1;
   guardToggle = -1;
   guardType = GUARDTYPE_NONE;
   desired = NODESTATE_INVALID;
   err = Thread::start();
   if( err ) return err;

   // Enable reception of node guarding messages
   err = Receiver::Init( canOpen, 0x700+nodeID );
   if( err ) return err;
   Receiver::EnableReceiver();

   // Init my SDO
   err = sdo.Init( canOpen, 0x600+nodeID, 0x580+nodeID );
   if( err ) return err;

   // Init the emergency object handler
   err = emcy.Init( this );
   if( err ) return err;

   // Enable the emergency receiver
   emcy.EnableReceiver();

   return 0;
}

/***************************************************************************/
/**
Un-initialize the Node object.  This puts the object back to it's default
state.
@return A pointer to an error object, or NULL on success.
*/
/***************************************************************************/
const Error *Node::UnInit( void )
{
   const Error *err = stop();

   if( !err ) err = Receiver::UnInit();
   if( !err ) err = sdo.UnInit();
   if( !err ) err = emcy.UnInit();
   return err;
}

/***************************************************************************/
/**
Associate the passed PDO object with this node.  The PDO
will be setup as this node's nth PDO.

@param slot Which PDO slot to assign this PDO to.
@param pdo The PDO object.
@param enable If true, the PDO will be enabled after being setup (default).
              If false, the PDO will be setup but not enabled.
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::PdoSet( uint16 slot, PDO &pdo, bool enable )
{
   // Make sure the slot is reasonable.
   if( slot > 511 )
      return &CanOpenError::BadParam;

   uint16 base = slot+pdo.BaseIndex();

   // Upload the current PDO ID from the
   // amplifier.  If the PDO is enabled, 
   // disable it now.
   uint32 pdoID;
   const Error *err = sdo.Upld32( base, 1, pdoID );
   if( err ) return err;

   if( !(pdoID & 0x80000000) )
   {
      pdoID |= 0x80000000;
      err = sdo.Dnld32( base, 1, pdoID );
      if( err ) return err;
   }

   // Find the ID code for this PDO
   uint32 newID = pdo.GetID() | 0x80000000;
   if( !pdo.GetRtrOk() ) newID |= 0x40000000;

   // Update the PDO ID if it's different.
   if( newID != pdoID )
   {
      err = sdo.Dnld32( base, 1, newID );
      if( err ) return err;
   }

   // Compare the new and old PDO type.
   // If different, update it.
   byte oldType;
   err = sdo.Upld8( base, 2, oldType );
   if( err ) return err;

   if( oldType != pdo.GetType() )
   {
      err = sdo.Dnld8( base, 2, pdo.GetType() );
      if( err ) return err;
   }

   // Update inhibit time & event time (not supported yet)

   // Get the PDO variable mapping info
   uint32 codes[PDO_MAP_LEN];
   byte ct = pdo.GetMapCodes( codes );

   base += 0x200;

   // Compare the new mapping info to the info already
   // in the PDO.  If it's different, then I'll update it.
   byte pdoCt;
   err = sdo.Upld8( base, 0, pdoCt );
   if( err ) return err;

   bool done = false;

   if( pdoCt == ct )
   {
      uint32 var;
      byte i;
      for( i=0; i<ct; i++ )
      {
	 err = sdo.Upld32( base, i+1, var );
	 if( err ) return err;

	 if( var != codes[i] )
	    break;
      }
      if( i == ct )
	 done = true;
   }

   if( !done )
   {
      // Clear out any old mapping
      err = sdo.Dnld8( base, 0, (byte)0 );
      if( err ) return err;

      // Download the new mapping
      for( byte i=0; i<ct; i++ )
      {
	 err = sdo.Dnld32( base, i+1, codes[i] );
	 if( err ) return err;
      }

      // Active the new mapping
      err = sdo.Dnld8( base, 0, ct );
      if( err ) return err;
   }

   // Enable the PDO if so requested
   if( enable )
      return PdoEnable( slot, pdo );

   return 0;
}

/***************************************************************************/
/**
Enable the specified PDO.  When this function is called it is assumed
that the PDO has already be setup.
@param n The slot number of the PDO to enable
@param base The index number in the object dictionary where this type
       of PDO starts (0x1800 for transmit, 0x1400 for receive)
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::PdoEnable( uint16 n, uint16 base )
{
   // Make sure the slot is reasonable.
   if( n > 511 )
      return &CanOpenError::BadParam;

   n += base;

   // Upload the PDO's ID
   uint32 id;
   const Error *err = sdo.Upld32( n, 1, id );
   if( err ) return err;

   // If it's not yet enabled, enable it
   if( id & 0x80000000 )
      err = sdo.Dnld32( n, 1, id&0x7fffffff );

   return err;
}

/***************************************************************************/
/**
Disable the specified PDO.
@param n The slot number of the transmit PDO to disable
@param base The index number in the object dictionary where this type
       of PDO starts (0x1800 for transmit, 0x1400 for receive)
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::PdoDisable( uint16 n, uint16 base )
{
   // Make sure the slot is reasonable.
   if( n > 511 )
      return &CanOpenError::BadParam;

   n += base;

   // Upload the PDO's ID
   uint32 id;
   const Error *err = sdo.Upld32( n, 1, id );
   if( err ) return err;

   // If it's enabled, disable it
   if( !(id & 0x80000000) )
      err = sdo.Dnld32( n, 1, (uint32)(id|0x80000000) );

   return err;
}

/***************************************************************************/
/**
Get the error history array (CANopen object 0x1003).

@param ct    When the function is first called, this variable holds the maximum number
             of errors that can be stored in the err array (i.e. the length of the array).
			    On return, the actual number of errors uploaded will be stored here.
@param array An array of 32-bit integers that will be used to return the list of errors.
@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *Node::GetErrorHistory( uint16 &ct, uint32 *array )
{
   uint32 i;

   // Upload the first element in the error array.
   // This holds the actual number of errors in it's 
   // lowest byte.
   const Error *err = sdo.Upld32( 0x1003, 0, i );
   if( err ) return err;

   if( i > 254 ) i = 254;

   // Limit the number of errors to download to ct
   if( i < (uint32)ct ) ct = i;

   for( i=1; i<=(uint32)ct; i++ )
   {
      err = sdo.Upld32( 0x1003, i, array[i-1] );
      if( err ) return err;
   }

   return 0;