sdo.cpp

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

   else if( size <= 0 )
      err = &CanOpenError::BadParam;

   else
   {
      // send an "Initiate SDO upload" message.
      frame.id = xmitID;
      frame.type = CAN_FRAME_DATA;
      frame.length = 8;

      frame.data[0] = 0x40;

      // Copy the object multiplexor to the frame
      // and also make a local copy.
      frame.data[1] = mplex[0] = ByteCast(index);
      frame.data[2] = mplex[1] = ByteCast(index>>8);
      frame.data[3] = mplex[2] = ByteCast(sub);

      // Clear out the reserved bytes
      for( int i=4; i<8; i++ )
	 frame.data[i] = 0;

      remain = size;

      // Keep a pointer to the data buffer and reset the toggle bit
      dataPtr = data;
      toggle = 0;

      state = SDO_STATE_SENT_UPLD_INIT;
      err = co->Xmit( frame, timeout );
      if( err ) state = SDO_STATE_IDLE;
   }

   // If the upload was started successfully, sit waiting for the
   // state to change to one that indicates success or failure.
   if( !err ) err = WaitForTransfer( size );
   mutex.Unlock();
   return err;
}

/***************************************************************************/
/**
Upload data using this SDO.  This function uses a block upload protocol
which makes sending large blocks of data more efficient.  The specified
object is upload from the CANopen node's object dictionary and stored in    
the array passed to this function.
 
@param index The index of the object to be uploaded.
@param sub The sub-index of the object to be uploaded.
@param size On entry, this should be the maximum number of bytes
       to upload, on successful return, this is the number of bytes
		 actually received.
@param data A character array which will store the uploaded data.
@return A valid CANopen error object.
*/
/***************************************************************************/
const Error *SDO::BlockUpld( int16 index, int16 sub, int32 &size, byte *data )
{
   CanFrame frame;
   const Error *err;

   // Make sure the SDO has been initialized
   if( !co ) return &CanOpenError::NotInitialized;

   mutex.Lock();

   if( state != SDO_STATE_IDLE )
      err = &CanOpenError::SDO_Busy;

   else if( size <= 0 )
      err = &CanOpenError::BadParam;

   else
   {
      // send an "Initiate block upload" message.
      frame.id = xmitID;
      frame.type = CAN_FRAME_DATA;
      frame.length = 8;

      // BUG: I don't support CRC checking at the moment
      frame.data[0] = 0xA0;

      // Copy the object multiplexor to the frame
      // and also make a local copy.
      frame.data[1] = mplex[0] = ByteCast(index);
      frame.data[2] = mplex[1] = ByteCast(index>>8);
      frame.data[3] = mplex[2] = ByteCast(sub);

      // For now, I use 127 segments/block and allow
      // drop back to normal update if the number of
      // actual bytes is less then my threshold.
      blockSize = 127;
      frame.data[4] = blockSize;
      frame.data[5] = SDO_BLK_UPLD_THRESHOLD-1;

      // Clear out the reserved bytes
      frame.data[6] = 0;
      frame.data[7] = 0;

      remain = size;
      toggle = 0;

      // Keep a pointer to the data buffer
      dataPtr = data;

      state = SDO_STATE_SENT_BUP_INIT;
      err = co->Xmit( frame, timeout );
      if( err ) state = SDO_STATE_IDLE;
   }

   if( !err ) err = WaitForTransfer( size );
   mutex.Unlock();
   return err;
}

/***************************************************************************/
/**
Download data using this SDO.  This function uses a block download protocol
which makes sending large blocks of data more efficient.  The data passed
to this function is downloaded to the object dictionary of the CANopen node

@param index The index of the object to be downloaded.
@param sub The sub-index of the object to be downloaded.
@param size The number of bytes of data to be downloaded
@param data A character array holding the data to be downloaded.
@return A valid CANopen error object.
*/
/***************************************************************************/
const Error *SDO::BlockDnld( int16 index, int16 sub, int32 size, byte *data )
{
   // This isn't supported yet, just return an error
   return &CanOpenError::NotSupported;
}

/***************************************************************************/
/**
Wait for the current SDO transfer to finish.  This is an internal function 
used by all the SDO transfer modes (upload, download, block upload, block
download).  It simply waits on the SDO's semaphore until either the SDO
transfer times out, or finishes either successfully or not.

@param size Used to pass back the size of the transfer.
@return An error object.
*/
/***************************************************************************/
const Error *SDO::WaitForTransfer( int32 &size )
{
   const Error *err;

   while( 1 )
   {
      err = sem.Get( timeout );

      if( err || state == SDO_STATE_DONE )
	 break;
   }

   if( err )
   {
      if( err == &ThreadError::Timeout )
	 err = &CanOpenError::SDO_Timeout;
      else
	 err = &CanOpenError::SDO_Unknown;

      SendAbort( SDO_ABORT_TIMEOUT );
   }

   else
   {
      err = lastErr;
      size = count;
   }

   state = SDO_STATE_IDLE;

   return err;
}

/***************************************************************************/
/**
Process a newly received CAN frame addressed to this SDO object.  This
is called from the CanOpen receive thread when a frame addressed to this
SDO is received over the network.  This frame is processed based on the
state of the SDO.
@param frame The frame to be processed
@return This function always returns 1.
*/
/***************************************************************************/
int SDO::NewFrame( CanFrame &frame )
{
   // Ignore messages when idle
   if( state == SDO_STATE_IDLE )
      return 1;

   // Ignore remote frames
   if( frame.type != CAN_FRAME_DATA )
      return 1;

   // Pull the server command specifier out of the message.
   // If no data was passed, then just set an illegal scs value.
   int16 scs = (frame.length<1) ? -1 : (int16)(7 & (frame.data[0]>>5));

   // This is filled in if we need to send an abort
   int32 abortCode = 0;

   /**************************************************
    * Handle block upload/download fall backs.  These
    * are responses to block up/downloads that treat
    * them as normal up/downloads.
    **************************************************/
   if( (state == SDO_STATE_SENT_BUP_INIT) && (scs==2) )
      state = SDO_STATE_SENT_UPLD_INIT;

   /**************************************************
    * Handle aborts.  An abort has an scs of 4, but 
    * for a block upload that could be valid, so check
    * for a byte value of 0x80 in that case.
    **************************************************/
   int isAbort = (frame.data[0]==0x80) || ( (scs==4) && (state!=SDO_STATE_SENT_BUP_START) );

   if( isAbort )
   {
      lastErr = getAbortRcvdErr( frame );

      // Either way, I'm done after receiving an abort.
      state = SDO_STATE_DONE;
      sem.Put();
      return 1;
   }

   /**************************************************
    * First, determine what to do based on my state
    * and the data contained in the message.
    **************************************************/
   switch( state )
   {
      /**************************************************
       * Handle the response to a upload init message
       **************************************************/
      case SDO_STATE_SENT_UPLD_INIT:
      {
	 // I expect an scs value of 2 (init upload response).
	 if( scs != 2 )
	 {
	    lastErr = &CanOpenError::SDO_BadMsgRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_BAD_SCS;
	    break;
	 }

	 int32 upldSize;

	 // Check the multiplexor sent with the message
	 int muxOK = (frame.data[1] == mplex[0] && 
	       frame.data[2] == mplex[1] && 
	       frame.data[3] == mplex[2]);

	 // If the multiplexor was not right, abort the transfer
	 if( !muxOK )
	 {
	    lastErr = &CanOpenError::SDO_BadMuxRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_GENERAL_ERR;
	    break;
	 }

	 // Grab the size information passed with 
	 // the message (if any is passed).
	 if( frame.data[0] & 1 )
	 {
	    // Expedited transfer size info
	    if( frame.data[0] & 2 )
	       upldSize = 4 - (3&(frame.data[0]>>2));

	    // Normal transfer size info
	    else
	    {
	       upldSize = bytes_to_int32( &frame.data[4] );
	    }
	 }

	 // If no size info was specified, just 
	 // set my size to -1.
	 else
	    upldSize = -1;

	 // If this is an expedited transfer, copy the data
	 // (as much as I can handle) to my buffer.
	 if( frame.data[0] & 2 )
	 {
	    int ct = (upldSize<0) ? 4 : upldSize;

	    // Clip the size to the available memory
	    if( ct > remain ) ct = remain;

	    for( int i=0; i<ct; i++ )
	       *dataPtr++ = frame.data[i+4];

	    remain -= ct;
	    count = ct;

	    lastErr = 0;
	    state = SDO_STATE_DONE;
	 }

	 // For normal transfers I'll send an upload
	 // request.  Note that I don't abort the 
	 // transfer if the upload size doesn't match
	 // my buffer size.  I'll just grab what ever
	 // data I can handle.
	 else
	 {
	    // If the upload size was specified, and is 
	    // less then my buffer size, I'll set my
	    // remaining count equal to it.
	    if( upldSize > 0 && upldSize < remain )
	       remain = upldSize;
	    state = SDO_STATE_SEND_UPLD;
	    count = 0;
	 }

	 break;
      }

      /**************************************************
       * Handle the response to an upload segment request
       **************************************************/
      case SDO_STATE_SENT_UPLD_RQST:
      {
	 // I expect an SCS of 0 indicating uploaded data.
	 if( scs != 0 )
	 {
	    lastErr = &CanOpenError::SDO_BadMsgRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_BAD_SCS;
	    break;
	 }

	 // Check the toggle bit
	 int expected = toggle ? 0 : 0x10;
	 if( (frame.data[0] & 0x10) != expected )
	 {
	    abortCode = SDO_ABORT_TOGGLEBIT;
	    lastErr = &SDO_Error::Togglebit;
	    state = SDO_STATE_SEND_ABORT;
	    break;
	 }

	 // If the number of bytes of data passed in this
	 // message was specified, then decode it.
	 // Otherwise, assume 7.
	 int ct = 7 - (7&(frame.data[0]>>1));

	 // If the number of bytes sent in the message is
	 // greater then my buffer size, clip it.
	 if( ct > remain ) ct = remain;

	 // Copy the data
	 for( int i=1; i<=ct; i++ )
	    *dataPtr++ = frame.data[i];

	 remain -= ct;
	 count += ct;

	 // If this was the last message then I'm done
	 if( frame.data[0] & 1 )
	 {
	    lastErr = 0;
	    state = SDO_STATE_DONE;
	 }

	 // Otherwise, if my buffer is full I'll
	 // abort the transfer.  We don't need your
	 // stinkin data!
	 else if( !remain )
	 {
	    lastErr = 0;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_TOO_LONG;
	 }

	 // I need more data, ask for some.
	 else
	    state = SDO_STATE_SEND_UPLD;

	 break;
      }

      /**************************************************