sdo.cpp

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

       * Handle the response to an download init message
       **************************************************/
      case SDO_STATE_SENT_DNLD_INIT:
      {
	 // Check the multiplexor sent with the message
	 int muxOK = (frame.data[1] == mplex[0] && 
	       frame.data[2] == mplex[1] && 
	       frame.data[3] == mplex[2]);

	 // I expect an scs value of 3 (init download response).
	 if( scs != 3 )
	 {
	    lastErr = &CanOpenError::SDO_BadMsgRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_BAD_SCS;
	    break;
	 }


	 // 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;
	 }

	 // If I have more data to send, then pass the next block
	 else if( remain > 0 )
	    state = SDO_STATE_SEND_DATA;

	 // Otherwise, I'm done
	 else
	 {
	    lastErr = 0;
	    state = SDO_STATE_DONE;
	 }

	 break;
      }

      /**************************************************
       * Handle the response to a download segment
       **************************************************/
      case SDO_STATE_SENT_DOWNLOAD:
      {
	 // I expect an scs value of 1 (download response).
	 if( scs != 1 )
	 {
	    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;
	 }

	 // If I have more data to send, then pass the next block
	 else if( remain > 0 )
	    state = SDO_STATE_SEND_DATA;

	 // Otherwise, I'm done
	 else
	 {
	    lastErr = 0;
	    state = SDO_STATE_DONE;
	 }

	 break;
      }

      /**************************************************
       * Handle the response to a block upload init.
       * Note that I don't have to worry about fall back 
       * to normal upload mode, I take care of that above.
       **************************************************/
      case SDO_STATE_SENT_BUP_INIT:
      {
	 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]);

	 // I expect an scs value of 6 (block upload response).
	 if( scs != 6 )
	 {
	    lastErr = &CanOpenError::SDO_BadMsgRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_BAD_SCS;
	    break;
	 }

	 // Make sure the sub-code was valid
	 if( frame.data[0] & 1 )
	 {
	    lastErr = &CanOpenError::SDO_BadMsgRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_BAD_SCS;
	    break;
	 }

	 // 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] & 2 )
	 {
	    upldSize = bytes_to_int32( &frame.data[4] );
	 }

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

	 // 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_BUP_START;
	 count = 0;

	 break;
      }

      /**************************************************
       * Handle a block upload.
       **************************************************/
      case SDO_STATE_SENT_BUP_START:
      {
	 // Find the sequence number for this segment
	 int seq = frame.data[0] & 0x7F;

	 // If this is my expected sequence number, 
	 // copy the data.
	 if( seq == lastBlkSeq+1 )
	 {
	    int ct = (remain<7) ? remain : 7;

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

	    remain -= ct;
	    count += ct;

	    lastBlkSeq++;
	 }

	 // If this was the last sequence, then send a response
	 if( frame.data[0] & 0x80 )
	 {
	    // If all data was received correctly, end the transfer
	    if( lastBlkSeq == seq )
	       state = SDO_STATE_SEND_BUP_LAST;

	    // Otherwise, re-request
	    else
	       state = SDO_STATE_SEND_BUP_NEXT;
	 }

	 // Send a response if this was the last
	 // segment in the block
	 else if( seq == blockSize )
	    state = SDO_STATE_SEND_BUP_NEXT;

	 break;
      }

      /**************************************************
       * All blocks have been received, waiting for end
       * message.
       **************************************************/
      case SDO_STATE_SENT_BUP_LAST:
      {
	 if( (scs != 6) || ((frame.data[0]&3) != 1) )
	 {
	    lastErr = &CanOpenError::SDO_BadMsgRcvd;
	    state = SDO_STATE_SEND_ABORT;
	    abortCode = SDO_ABORT_BAD_SCS;
	    break;
	 }

	 lastErr = 0;
	 state = SDO_STATE_SEND_BUP_DONE;
	 break;
      }
   }

   /**************************************************
    * Now that I've figured out what to do (and updated
    * the state variable to indicate what it should be)
    * just do it!
    **************************************************/
   frame.id = xmitID;
   frame.length = 8;

   switch( state )
   {
      case SDO_STATE_SEND_ABORT:
	 {
	    // Send an abort frame 
	    SendAbort( abortCode );

	    // Now, update my state to done.
	    state = SDO_STATE_DONE;

	    // Fall through to done processing
	 }

      case SDO_STATE_DONE:
	 // Indicate that I've finished.
	 sem.Put();
	 break;

      case SDO_STATE_SEND_DATA:
	 {
	    // Send the next data block
	    int ct = (remain > 7 ) ? 7 : remain;
	    remain -= ct;

	    frame.data[0] = (7-ct)<<1;
	    if( toggle )  frame.data[0] |= 0x10;
	    if( !remain ) frame.data[0] |= 0x01;

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

	    toggle = !toggle;
	    state = SDO_STATE_SENT_DOWNLOAD;
	    co->Xmit( frame, timeout );
	    break;
	 }

      case SDO_STATE_SEND_UPLD:
	 {
	    frame.data[0] = toggle ? 0x70 : 0x60;

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

	    toggle = !toggle;
	    state = SDO_STATE_SENT_UPLD_RQST;
	    co->Xmit( frame, timeout );
	    break;
	 }

      case SDO_STATE_SEND_BUP_START:
	 {
	    frame.data[0] = 0xA3;
	    for( int i=1; i<8; i++ )
	       frame.data[i] = 0;

	    lastBlkSeq = 0;
	    state = SDO_STATE_SENT_BUP_START;
	    co->Xmit( frame, timeout );
	    break;
	 }

      case SDO_STATE_SEND_BUP_NEXT:
      case SDO_STATE_SEND_BUP_LAST:
	 {
	    frame.data[0] = 0xA2;
	    frame.data[1] = ByteCast(lastBlkSeq);
	    blockSize = 127;
	    frame.data[2] = blockSize;

	    for( int i=3; i<8; i++ )
	       frame.data[i] = 0;

	    if( state == SDO_STATE_SEND_BUP_LAST )
	       state = SDO_STATE_SENT_BUP_LAST;
	    else
	       state = SDO_STATE_SENT_BUP_START;

	    lastBlkSeq = 0;
	    co->Xmit( frame, timeout );
	    break;
	 }

      case SDO_STATE_SEND_BUP_DONE:
	 {
	    frame.data[0] = 0xA1;
	    for( int i=1; i<8; i++ )
	       frame.data[i] = 0;

	    co->Xmit( frame, timeout );
	    state = SDO_STATE_DONE;
	    sem.Put();
	    break;
	 }
   }

   return 1;
}

/***************************************************************************/
/**
Send an abort frame.
*/
/***************************************************************************/
void SDO::SendAbort( int32 abortCode )
{
   // Make sure the SDO has been initialized
   if( !co ) return;

   CanFrame frame;

   frame.id = xmitID;
   frame.type = CAN_FRAME_DATA;
   frame.length = 8;
   frame.data[0] = 0x80;
   frame.data[1] = mplex[0];
   frame.data[2] = mplex[1];
   frame.data[3] = mplex[2];
   frame.data[4] = ByteCast(abortCode);
   frame.data[5] = ByteCast(abortCode>>8);
   frame.data[6] = ByteCast(abortCode>>16);
   frame.data[7] = ByteCast(abortCode>>24);

   co->Xmit( frame, timeout );
}

/***************************************************************************/
/**
Translate the passed SDO abort code into an error message.
*/
/***************************************************************************/
const Error *SDO::getAbortRcvdErr( CanFrame &frame )
{
   int32 code = bytes_to_int32( &frame.data[4] );

   switch( code )
   {
      case 0:                                return &SDO_Error::NoAbortCode;
      case SDO_ABORT_TOGGLEBIT:              return &SDO_Error::Togglebit;
      case SDO_ABORT_TIMEOUT:                return &SDO_Error::Timeout;
      case SDO_ABORT_BAD_SCS:                return &SDO_Error::Bad_scs;
      case SDO_ABORT_BLOCK_SIZE:             return &SDO_Error::Block_size;
      case SDO_ABORT_BLOCK_SEQ:              return &SDO_Error::Block_seq;
      case SDO_ABORT_BLOCK_CRC:              return &SDO_Error::Block_crc;
      case SDO_ABORT_MEMORY:                 return &SDO_Error::Memory;
      case SDO_ABORT_ACCESS:                 return &SDO_Error::Access;
      case SDO_ABORT_WRITEONLY:              return &SDO_Error::Writeonly;
      case SDO_ABORT_READONLY:               return &SDO_Error::Readonly;
      case SDO_ABORT_BAD_OBJECT:             return &SDO_Error::Bad_object;
      case SDO_ABORT_PDO_MAP:                return &SDO_Error::Pdo_map;
      case SDO_ABORT_PDO_LENGTH:             return &SDO_Error::Pdo_length;
      case SDO_ABORT_BAD_PARAM:              return &SDO_Error::Bad_param;
      case SDO_ABORT_INCOMPATIBLE:           return &SDO_Error::Incompatible;
      case SDO_ABORT_HARDWARE:               return &SDO_Error::Hardware;
      case SDO_ABORT_BAD_LENGTH:             return &SDO_Error::Bad_length;
      case SDO_ABORT_TOO_LONG:               return &SDO_Error::Too_long;
      case SDO_ABORT_TOO_SHORT:              return &SDO_Error::Too_short;
      case SDO_ABORT_SUBINDEX:               return &SDO_Error::Subindex;
      case SDO_ABORT_PARAM_RANGE:            return &SDO_Error::Param_range;
      case SDO_ABORT_PARAM_HIGH:             return &SDO_Error::Param_high;
      case SDO_ABORT_PARAM_LOW:              return &SDO_Error::Param_low;
      case SDO_ABORT_MIN_MAX:                return &SDO_Error::Min_max;
      case SDO_ABORT_GENERAL_ERR:            return &SDO_Error::General;
      case SDO_ABORT_TRANSFER:               return &SDO_Error::Transfer;      
      case SDO_ABORT_TRANSFER_LOCAL:         return &SDO_Error::Transfer_Local;
      case SDO_ABORT_TRANSFER_STATE:         return &SDO_Error::Transfer_State;
      case SDO_ABORT_OD_GEN_FAIL:            return &SDO_Error::OD_Gen_Fail;
      default:                               return &SDO_Error::Unknown;
   }
}