pmdser.c

一个机器人的源代码.软件设计得超级好!是商业级代码.

      STOPBITS = CSTOPB;
      break;
  }  //end of switch stop bits

  /** @Warning CMSPAR for setting Mark/Space parity is not portable */
  switch ( p )
  {
    case NOPARITY:
    default:                       //none
      PARITYON = 0;
      PARITY = 0;
      break;
    case ODDPARITY:                        //odd
      PARITYON = PARENB;
      PARITY = PARODD;
      break;
    case EVENPARITY:                        //even
      PARITYON = PARENB;
      PARITY = 0;
      break;
    case MARKPARITY:                        //mark
      PARITYON = PARENB;
      PARITY = PARODD | CMSPAR;
      break;
    case SPACEPARITY:                        //space
      PARITYON = PARENB;
      PARITY = CMSPAR;
      break;
  }  //end of switch parity
       
  
  // set new port settings
  newtio.c_cflag = 0;

  cfsetospeed( &newtio, BAUD );
  cfsetispeed( &newtio, BAUD );

  newtio.c_cflag |= CS8 | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
  newtio.c_iflag = IGNPAR;
  newtio.c_oflag = 0;
  newtio.c_lflag = 0;
  newtio.c_cc[VMIN] = 0;
  newtio.c_cc[VTIME] = 1;

  tcflush( SIOtransport_data->hPort, TCIFLUSH);
  
  tcsetattr( SIOtransport_data->hPort, TCSANOW, &newtio);

  if ( customBaud == 1 )
  {
    struct serial_struct serial_info;

    // FTDI chip has a 6MHz clock.
    // the baud clock divisor = 6MHz/desired baudrate.
    // round off to get the closest divisor.
    serial_info.flags = ASYNC_SPD_CUST | ASYNC_LOW_LATENCY;
    serial_info.custom_divisor = (float)24000000/(float)b + 0.5;

    if ( ioctl( SIOtransport_data->hPort, TIOCSSERIAL, &serial_info ) < 0)
    {
      perror("config_serial_port: ioctl TIOCSSERIAL");
      return(-1);
    }
  }
#endif

  return TRUE;
}

// ------------------------------------------------------------------------
BOOL PMDSerial_SetTimeout(void* transport_data,long msec)
{
  PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;

  if( SIOtransport_data->hPort == INVALID_HANDLE_VALUE )
    return FALSE;
  
  return ConfigPortTimeouts( SIOtransport_data->hPort, 10, msec );
}

// ------------------------------------------------------------------------
void PMDSerial_SetProtocol(void* transport_data,PMDuint16 mode)
{
	PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;

	if (mode != Protocol_ModeAddressBit)
		PMDSerial_SetConfig( transport_data, SIOtransport_data->baud, NOPARITY );
	SIOtransport_data->protocol = mode;
}

// ------------------------------------------------------------------------
void PMDSerial_SetMultiDropAddress(void* transport_data,PMDuint16 address)
{
	PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;

	SIOtransport_data->multiDropAddress = address;
}

void Log( const char* str )
{
  FILE* fp = fopen( "foo.log", "a" );
  fprintf( fp, "%s\n", str );
  fclose( fp );
}

// ------------------------------------------------------------------------
PMDresult PMDSerial_Send(void* transport_data, 
			 PMDuint8 xCt, PMDuint16* xDat,
			 PMDuint8 rCt, PMDuint16* rDat)
{
  PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;

  unsigned int c=0;
  unsigned int i;
  int sum = 0;
  unsigned long bytes;
  PMDuint8 buffer[40];
  PMDuint16 ProcessorError;
  int bytesToRead;

  if( !SIOtransport_data || (SIOtransport_data->hPort == INVALID_HANDLE_VALUE ))
    return PMD_ERR_NotConnected;

  /* Clear address byte & checksum byte */
  buffer[ c++ ] = (char)( (SIOtransport_data->multiDropAddress) &0x1f);
  buffer[ c++ ] = (char)(0);

  /* Add axis number and command code */
  buffer[ c++ ] = (char)(xDat[0]>>8);
  buffer[ c++ ] = (char)(xDat[0]&0xff);

  /* add data (handling byte swapping) */
  for( i=1; i<xCt; i++ )
  {
    buffer[ c++ ] = (char)(xDat[i] >> 8);
    buffer[ c++ ] = (char)(xDat[i] & 0xFF);
  }

  /* calculate checksum */
  for( i=0; i<c; i++ )
  {
    sum += buffer[i];
  }

  buffer[1] = (~(sum & 0xFF))+1;

  // Flush the receive buffer in case any unexpected bytes have been received
  PMDSerial_FlushRecv(transport_data);

  // if we are using address bit, fake the address bit using the
  // parity bit
  if (SIOtransport_data->protocol == Protocol_ModeAddressBit)
  {
    if ( !FlushPort( SIOtransport_data->hPort ))
      return PMD_ERR_CommPortWrite;

    PMDSerial_SetConfig(transport_data, SIOtransport_data->baud, MARKPARITY);

    if (!WritePort( SIOtransport_data->hPort, buffer, 1, &bytes ) || bytes != 1 )
      return PMD_ERR_CommPortWrite;

    if ( !FlushPort( SIOtransport_data->hPort ))
      return PMD_ERR_CommPortWrite;

    SLEEP(1);
    
    PMDSerial_SetConfig( transport_data, SIOtransport_data->baud, SPACEPARITY );
    SLEEP(1);
	
    if( !WritePort( SIOtransport_data->hPort, buffer+1, c-1, &bytes ) || bytes != c-1 )
      return PMD_ERR_CommPortWrite;
  }
  else if( !WritePort( SIOtransport_data->hPort, buffer, c, &bytes ) || bytes != c )
  {
    //Log( "WritePort returned false" );
    
    return PMD_ERR_CommPortWrite;
  }

  /* read return data */
  bytesToRead = 2*rCt+2;

  if( !ReadPort( SIOtransport_data->hPort, buffer, bytesToRead, &bytes ) )
  {
    //  Log( "ReadPort returned false" );
    
    return PMD_ERR_CommTimeoutError;
  }

  // if there was an error that was not a reset, then
  // don't attempt to receive any data
  if(( buffer[0] & ~(PMD_ERR_Reset)) && bytes==2 )
  {
    //  Log( "Only got 2 received bytes" );

    rCt = 0;
  }
 
  // if we don't get enough bytes
  if( bytes != (unsigned)(bytesToRead) )
  {
    return PMD_ERR_CommPortRead;
  }

  // verify the checksum
  for( sum=i=0; i<bytes; i++ )
    sum += buffer[i];

  if( sum & 0xFF )
  {
    return PMD_ERR_ChecksumError;
  }

  /* byte swap return data */
  for( i=0, c=2; i<rCt; i++ )
  {
    rDat[i]  = (PMDuint16)((buffer[c++])<<8);
    rDat[i] |= (PMDuint16)(buffer[c++]);
  }

  // first byte is a chipset error code 
  ProcessorError = buffer[0];

  if ( ProcessorError && (ProcessorError != PMD_ERR_Reset) )
  {
    if ( SIOtransport_data->bDiagnostics )
    {
      char buf[160];
      sprintf( buf, "Processor status: 0x%X %s\n", ProcessorError, PMDGetErrorMessage(ProcessorError));
      PMDprintf( buf );
      sprintf( buf, "C-Motion: %s\n",PMDGetOpcodeText(xDat[0]));
      PMDprintf( buf );

      strcpy( buf, " TXdata: " );
      for(i=0; i<xCt; i++)
      {
	sprintf( buf + strlen( buf ), "%04X ", xDat[i]);
      }
      PMDprintf( "%s\n", buf );

      strcpy( buf, " RXdata: " ); 
      for(i=0; i<rCt; i++)
      {
	sprintf( buf + strlen( buf ), "%04X ", rDat[i]);
      }
      PMDprintf( "%s\n", buf );
    }
  }

  // serial port may be out of sync if an instruction error occured
  //if (SIOtransport_data->bDiagnostics && ProcessorError != PMD_ERR_OK)
  //    PMDSerial_Sync(transport_data);

  return ( ProcessorError );
}

// ------------------------------------------------------------------------
PMDuint16 PMDSerial_WriteByte(void* transport_data, char data)
{
  PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;
  unsigned long bytes;

  if( !WritePort( SIOtransport_data->hPort, &data, 1, &bytes ) || bytes != 1 )
    return PMD_ERR_CommPortWrite;

  return PMD_ERR_OK;
}

// ------------------------------------------------------------------------
PMDuint16 PMDSerial_ReadByte(void* transport_data, PMDuint8* data)
{
  PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;
  unsigned long bytes;

  if( !ReadPort( SIOtransport_data->hPort, (char*)data, 1, &bytes ) || bytes != 1 )
    return PMD_ERR_CommPortRead;
  return PMD_ERR_OK;
}

// ------------------------------------------------------------------------
PMDuint16 PMDSerial_Sync(void* transport_data)
{
  PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;
  const int maxSend = 15;
  unsigned long bytes;
  int i;

  if( SIOtransport_data->hPort == INVALID_HANDLE_VALUE ) return PMD_ERR_NotConnected;

  // sync is not required with multi-drop protocols because the command buffer resets
  // after idle time or address bit is received.
  if (SIOtransport_data->protocol != Protocol_PointToPoint)
    return PMD_ERR_InvalidOperation;

  // Flush the receive buffer in case any unexpected bytes have been received
  PMDSerial_FlushRecv(transport_data);

  for( i=0; i<maxSend; i++ )
  {
    char ch = 0;

    /* write a zero character */
    if( !WritePort( SIOtransport_data->hPort, &ch, 1, &bytes ) || bytes != 1 )
      return PMD_ERR_CommPortWrite;

    /* Attemp to read a response */
    if( ReadPort( SIOtransport_data->hPort, &ch, 1, &bytes ) && bytes==1 )
      break;
  }

  /* If no data was seen, return an error */
  if( i== maxSend )
    return PMD_ERR_CommTimeoutError;

  /* flush any other data read */
  PMDSerial_FlushRecv(transport_data);

  return PMD_ERR_OK;
}

// ------------------------------------------------------------------------
PMDuint16 PMDSerial_FlushRecv(void* transport_data)
{
  PMDSerialIOData* SIOtransport_data = (PMDSerialIOData*)transport_data;

  if( !SIOtransport_data || (SIOtransport_data->hPort == INVALID_HANDLE_VALUE) )
    return PMD_ERR_NotConnected;

  FlushRxPort( SIOtransport_data->hPort );

  return PMD_ERR_OK;
}

// ------------------------------------------------------------------------
void PMDSerial_InitData(PMDSerialIOData* transport_data)
{
	// assign default values
	transport_data->multiDropAddress = 0;
	transport_data->protocol = Protocol_PointToPoint;
	transport_data->baud = g_deviceBaudRate;

	// default port is COM1
	transport_data->port = 1;

	// by default always verify the checksum
	transport_data->bVerifyChecksum = 1;
	// by default disable diagnostics
	transport_data->bDiagnostics = 0;
}

// ------------------------------------------------------------------------
PMDuint16 PMDSerial_Init(PMDAxisHandle* handle)
{
	PMDAxisHandle* axis_handle = (PMDAxisHandle*) handle;

	// setup function pointers
	axis_handle->transport.SendCommand = PMDSerial_Send;

	axis_handle->transport.GetStatus = PMDSerial_GetStatus;
	axis_handle->transport.IsReady = PMDSerial_IsReady;
	axis_handle->transport.HasInterrupt = PMDSerial_HasInterrupt;
	axis_handle->transport.HasError = PMDSerial_HasError;
	axis_handle->transport.HardReset = PMDSerial_HardReset;

	axis_handle->transport.Close = PMDSerial_Close;

	return PMDSerial_InitPort(axis_handle->transport_data);
}

// ------------------------------------------------------------------------
// Use this function to copy an existing handle to an open port 
// and set the multi-drop address and axis number.
void PMDCreateMultiDropHandle(PMDAxisHandle* dest_axis_handle, PMDAxisHandle* src_axis_handle, PMDAxis axis_number, PMDuint16 address)
{
	PMDSerialIOData* transport_data;

    // copy the handle
	memcpy( dest_axis_handle, src_axis_handle, sizeof( PMDAxisHandle ) );
    // allocate a new transport_data structure because the multi-drop address will be different for each axis handle
    transport_data = (PMDSerialIOData*) malloc( sizeof( PMDSerialIOData ) );
    dest_axis_handle->transport_data = transport_data;
	memcpy(dest_axis_handle->transport_data, src_axis_handle->transport_data, sizeof(PMDSerialIOData));

	// set the axis number and multi-drop address 
	dest_axis_handle->axis = axis_number;
    transport_data->multiDropAddress = address;
}   

/*****************************************************************************
Set port_number to COMn port number (1 = COM1, 0 = default)
/*****************************************************************************/
PMDuint16 PMDSetupAxisInterface_Serial(PMDAxisHandle* axis_handle, PMDAxis axis_number, PMDuint8 port_number, PMDuint8 deviceType)
{
	PMDSerialIOData* transport_data;

	/* Set the baud rate according to which
	   device we are connecting to. RCM or M3
	 */
	if ( deviceType == DEVICE_TYPE_RCM )
	{
          g_deviceBaudRate = 250000;
	}
	else
	{ // DEVICE_TYPE_M3
	  g_deviceBaudRate = 416667;
	}

	transport_data = (PMDSerialIOData*) malloc( sizeof( PMDSerialIOData ) );
	memset(transport_data, 0, sizeof(PMDSerialIOData));

	// set the axis we are talking to with this handle
	axis_handle->axis = axis_number;

	// set the interface type 
	axis_handle->InterfaceType = InterfaceSerial;

	// the transport data is initialized first to setup the defaults
	PMDSerial_InitData(transport_data);

	// assign port number that this axis handle will use
	// if 0 use default (COM1)
	//	if (port_number)
	transport_data->port = port_number;

	axis_handle->transport_data = (void*) transport_data;

	// initialize the transport (inits function pointers)
	return PMDSerial_Init(axis_handle);
}