sdo.cpp

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

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

/**
\file

This file contains the code used to implement the CANopen SDO objects.
*/

#include "CML.h"

CML_NAMESPACE_USE();

/**************************************************
* SDO Error objects
**************************************************/
CML_NEW_ERROR( SDO_Error, NoAbortCode,       "SDO Aborted" );
CML_NEW_ERROR( SDO_Error, Togglebit,         "SDO Abort: Toggle bit not alternated." );
CML_NEW_ERROR( SDO_Error, Timeout,           "SDO Abort: SDO Protocol timed out." );
CML_NEW_ERROR( SDO_Error, Bad_scs,           "SDO Abort: Client/Server command specifier not known." );
CML_NEW_ERROR( SDO_Error, Block_size,        "SDO Abort: Invalid block size." );
CML_NEW_ERROR( SDO_Error, Block_seq,         "SDO Abort: Invalid block sequence." );
CML_NEW_ERROR( SDO_Error, Block_crc,         "SDO Abort: CRC error." );
CML_NEW_ERROR( SDO_Error, Memory,            "SDO Abort: Memory allocation error." );
CML_NEW_ERROR( SDO_Error, Access,            "SDO Abort: Unsupported object access." );
CML_NEW_ERROR( SDO_Error, Writeonly,         "SDO Abort: Object is write only." );
CML_NEW_ERROR( SDO_Error, Readonly,          "SDO Abort: Object is read only." );
CML_NEW_ERROR( SDO_Error, Bad_object,        "SDO Abort: Object does not exist." );
CML_NEW_ERROR( SDO_Error, Pdo_map,           "SDO Abort: Object can not be mapped to PDO." );
CML_NEW_ERROR( SDO_Error, Pdo_length,        "SDO Abort: PDO length would be exceeded." );
CML_NEW_ERROR( SDO_Error, Bad_param,         "SDO Abort: General parameter error." );
CML_NEW_ERROR( SDO_Error, Incompatible,      "SDO Abort: General internal incompatibility." );
CML_NEW_ERROR( SDO_Error, Hardware,          "SDO Abort: Hardware failure." );
CML_NEW_ERROR( SDO_Error, Bad_length,        "SDO Abort: Data length incorrect." );
CML_NEW_ERROR( SDO_Error, Too_long,          "SDO Abort: Data length too long." );
CML_NEW_ERROR( SDO_Error, Too_short,         "SDO Abort: Data length too short." );
CML_NEW_ERROR( SDO_Error, Subindex,          "SDO Abort: Sub-index does not exist." );
CML_NEW_ERROR( SDO_Error, Param_range,       "SDO Abort: Parameter range error." );
CML_NEW_ERROR( SDO_Error, Param_high,        "SDO Abort: Parameter value too high." );
CML_NEW_ERROR( SDO_Error, Param_low,         "SDO Abort: Parameter value too low." );
CML_NEW_ERROR( SDO_Error, Min_max,           "SDO Abort: Maximum value less then minimum." );
CML_NEW_ERROR( SDO_Error, General,           "SDO Abort: General error." );
CML_NEW_ERROR( SDO_Error, Transfer,          "SDO Abort: Data transfer error." );
CML_NEW_ERROR( SDO_Error, Transfer_Local,    "SDO Abort: Data transfer error; local control." );
CML_NEW_ERROR( SDO_Error, Transfer_State,    "SDO Abort: Data transfer error; device state." );
CML_NEW_ERROR( SDO_Error, OD_Gen_Fail,       "SDO Abort: Object dictionary generation failure." );
CML_NEW_ERROR( SDO_Error, Unknown,           "SDO Abort: Unknown abort code" );


/**************************************************
* States used internally by the SDO object
**************************************************/
#define SDO_STATE_IDLE                   0     // SDO is idle
#define SDO_STATE_DONE                   1     // SDO transfer finished
#define SDO_STATE_SENT_DNLD_INIT         2     // SDO download init was sent.
#define SDO_STATE_SENT_DOWNLOAD          3     // SDO download sent
#define SDO_STATE_SENT_UPLD_INIT         4     // SDO upload init was sent.
#define SDO_STATE_SENT_UPLD_RQST         5     // SDO upload request sent
#define SDO_STATE_SENT_BUP_INIT          6     // Block upload init sent
#define SDO_STATE_SENT_BUP_START         7     // Block upload data expected
#define SDO_STATE_SENT_BUP_LAST          8     // Block upload done

#define SDO_STATE_SEND_ABORT            10     // I need to send an abort
#define SDO_STATE_SEND_DATA             11     // I need to send more data
#define SDO_STATE_SEND_UPLD             12     // I need to send an upload request
#define SDO_STATE_SEND_BUP_START        13     // I need to send a block upload start
#define SDO_STATE_SEND_BUP_NEXT         14     // request next block upload block
#define SDO_STATE_SEND_BUP_LAST         15     // Last block has been uploaded
#define SDO_STATE_SEND_BUP_DONE         16     // Upload finished, send confirmation

/***************************************************************************/
/**
Initialize the CANopen Service Data Object (SDO).
@param canOpen The CANopen network object that this SDO is associated with
@param xmit The COB ID used when transmitting SDO frames
@param recv The COB ID used for receive frames
@param to The timeout (milliseconds) for use with this SDO.
*/
/***************************************************************************/
SDO::SDO( CanOpen &canOpen, uint32 xmit, uint32 recv, int32 to )
{
   Init( canOpen, xmit, recv, to );
}

/***************************************************************************/
/**
Initialize a CANopen Service Data Object (SDO).
@param canOpen The CANopen network object that this SDO is associated with
@param xmit The COB ID used when transmitting SDO frames
@param recv The COB ID used for receive frames
@param to The timeout (milliseconds) for use with this SDO.
@return A valid CANopen error object
*/
/***************************************************************************/
const Error *SDO::Init( CanOpen &canOpen, uint32 xmit, uint32 recv, int32 to)
{
   xmitID = xmit;
   state = SDO_STATE_IDLE;
   timeout = to;
   lastErr = 0;

   blkUpldOK = false;
   blkDnldOK = false;

   const Error *err = Receiver::Init( canOpen, recv );
   if( err ) return err;
   EnableReceiver();

   return 0;
}

/***************************************************************************/
/**
Download a 32-bit value using this SDO.
@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data The data to be downloaded
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::Dnld32( int16 index, int16 sub, uint32 data )
{
   byte buff[4];
   buff[0] = ByteCast(data);
   buff[1] = ByteCast(data>>8);
   buff[2] = ByteCast(data>>16);
   buff[3] = ByteCast(data>>24);
   return Download( index, sub, 4, buff );
}

/***************************************************************************/
/**
Upload a 32-bit value using this SDO.
@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data The uploaded data will be returned here.
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::Upld32( int16 index, int16 sub, uint32 &data )
{
   int32 size = 4;
   byte buff[4];
   buff[0] = buff[1] = buff[2] = buff[3] = 0;

   const Error *err = Upload( index, sub, size, buff );

   data = bytes_to_uint32( buff );

   return err;
}

/***************************************************************************/
/**
Download a 16-bit value using this SDO.
@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data The data to be downloaded
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::Dnld16( int16 index, int16 sub, uint16 data )
{
   byte buff[2];
   buff[0] = ByteCast(data);
   buff[1] = ByteCast(data>>8);
   return Download( index, sub, 2, buff );
}

/***************************************************************************/
/**
Upload a 16-bit value using this SDO.
@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data The uploaded data will be returned here.
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::Upld16( int16 index, int16 sub, uint16 &data )
{
   int32 size = 2;
   byte buff[2];
   buff[0] = buff[1] = 0;

   const Error *err = Upload( index, sub, size, buff );

   data = bytes_to_uint16(buff);
   return err;
}

/***************************************************************************/
/**
Download a 8-bit value using this SDO.
@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data The data to be downloaded
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::Dnld8( int16 index, int16 sub, uint8 data )
{
   return Download( index, sub, 1, &data );
}

/***************************************************************************/
/**
Upload a 8-bit value using this SDO.
@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data The uploaded data will be returned here.
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::Upld8( int16 index, int16 sub, uint8 &data )
{
   int32 size = 1;
   return Upload( index, sub, size, &data );
}

/***************************************************************************/
/**
Download a visible string type using the SDO.  The string is assumed
to be null terminated.

@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param data A null terminated string to be downloaded.
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::DnldString( int16 index, int16 sub, char *data )
{
   // Find the string length
   int32 i;
   for( i=0; data[i]; i++ );

   return Download( index, sub, i, data );
}

/***************************************************************************/
/**
Upload a visible string type from the SDO.  The only difference between
this function and the lower level Upload function is that this function
guarantees that there will be a zero character at the end of the string.

@param index The index of the object in the object dictionary
@param sub The sub-index of the object in the object dictionary
@param len Holds the size of the buffer on entry, and the
           length of the downloaded data on return.
@param data The uploaded string will be returned here.
@return A valid CANopen error code.
*/
/***************************************************************************/
const Error *SDO::UpldString( int16 index, int16 sub, int32 &len, char *data )
{
   len--;

   const Error *err = Upload( index, sub, len, data );
   if( err ) return err;

   data[len] = 0;
   return 0;
}

/***************************************************************************/
/**
Download data using this SDO.  The passed array of data is downloaded to the
object dictionary of a node on the CANopen network using this SDO.
@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::Download( int16 index, int16 sub, int32 size, byte *data )
{
   CanFrame frame;
   const Error *err;

   // Use a block download if it makes sense to do so
   if( blkDnldOK && size >= SDO_BLK_DNLD_THRESHOLD )
      return BlockDnld( index, sub, size, data );

   // 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 SDO download" message.
      frame.id = xmitID;
      frame.type = CAN_FRAME_DATA;
      frame.length = 8;

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

      // If the data size is <= 4 bytes, then send an expedited download
      if( size <= 4 )
      {
	 frame.data[0] = 0x23 | ((4-size)<<2);

	 int32 i;
	 for( i=0; i<size; i++ ) frame.data[i+4] = ByteCast(data[i]);
	 for( ; i<4; i++ )       frame.data[i+4] = 0;

	 remain = 0;
      }

      // Otherwise, send a normal init
      else
      {
	 frame.data[0] = 0x21;
	 frame.data[4] = ByteCast(size);
	 frame.data[5] = ByteCast(size>>8);
	 frame.data[6] = ByteCast(size>>16);
	 frame.data[7] = ByteCast(size>>24);

	 remain = size;
      }

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

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

   // If the download 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.  The value of the object is uploaded from the
object dictionary of a node on the CANopen network using this SDO.  The 
results of the upload are stored in the passed buffer.

@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 gives the maximum number of bytes of data to 
       be uploaded.  On successful return, it gives the actual number of
		 bytes received.
@param data A character array which will store the uploaded data.
@return A valid CANopen error object.
*/
/***************************************************************************/
const Error *SDO::Upload( int16 index, int16 sub, int32 &size, byte *data )
{
   CanFrame frame;
   const Error *err;

   // Use a block upload if it makes sense to do so
   if( blkUpldOK && size >= SDO_BLK_UPLD_THRESHOLD )
      return BlockUpld( index, sub, size, data );

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

   mutex.Lock();

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