Exemple #1
0
void* DR::EMHandler::thread_connect()
{
	int	errorCode;

	m_lock.lock();

	

	// Initialize the ATC3DG driver and DLL
	//
	// It is always necessary to first initialize the ATC3DG "system". By
	// "system" we mean the set of ATC3DG cards installed in the PC. All cards
	// will be initialized by a single call to InitializeBIRDSystem(). This
	// call will first invoke a hardware reset of each board. If at any time
	// during operation of the system an unrecoverable error occurs then the
	// first course of action should be to attempt to Recall InitializeBIRDSystem()
	// if this doesn't restore normal operating conditions there is probably a
	// permanent failure - contact tech support.
	// A call to InitializeBIRDSystem() does not return any information.
	//
	
	errorCode = InitializeBIRDSystem();
	if (errorCode != BIRD_ERROR_SUCCESS)
	{
		errorHandler(errorCode);
		m_lock.unlock();
		return 0;
	}
	BOOL metric = true;
	errorCode = SetSystemParameter(METRIC, &metric, sizeof(metric));
	if (errorCode != BIRD_ERROR_SUCCESS) errorHandler(errorCode);
	// GET SYSTEM CONFIGURATION
	//
	// In order to get information about the system we have to make a call to
	// GetBIRDSystemConfiguration(). This call will fill a fixed size structure
	// containing amongst other things the number of boards detected and the
	// number of sensors and transmitters the system can support (Note: This
	// does not mean that all sensors and transmitters that can be supported
	// are physically attached)
	//

	errorCode = GetBIRDSystemConfiguration(&ATC3DG.m_config);
	if (errorCode != BIRD_ERROR_SUCCESS)
	{
		errorHandler(errorCode);
		m_lock.unlock();
		return 0;
	}

	// GET SENSOR CONFIGURATION
	//
	// Having determined how many sensors can be supported we can dynamically
	// allocate storage for the information about each sensor.
	// This information is acquired through a call to GetSensorConfiguration()
	// This call will fill a fixed size structure containing amongst other things
	// a status which indicates whether a physical sensor is attached to this
	// sensor port or not.
	//

	pSensor = new CSensor[ATC3DG.m_config.numberSensors];
	for (int i = 0; i < ATC3DG.m_config.numberSensors; i++)
	{
		errorCode = GetSensorConfiguration(i, &(pSensor + i)->m_config);
		if (errorCode != BIRD_ERROR_SUCCESS)
		{
			errorHandler(errorCode);
			m_lock.unlock();
			return 0;
		}
	}

	// GET TRANSMITTER CONFIGURATION
	//
	// The call to GetTransmitterConfiguration() performs a similar task to the
	// GetSensorConfiguration() call. It also returns a status in the filled
	// structure which indicates whether a transmitter is attached to this
	// port or not. In a single transmitter system it is only necessary to
	// find where that transmitter is in order to turn it on and use it.
	//
	
	pXmtr = new CXmtr[ATC3DG.m_config.numberTransmitters];
	for (int i = 0; i < ATC3DG.m_config.numberTransmitters; i++)
	{
		errorCode = GetTransmitterConfiguration(i, &(pXmtr + i)->m_config);
		if (errorCode != BIRD_ERROR_SUCCESS)
		{
			errorHandler(errorCode);
			m_lock.unlock();
			return 0;
		}
	}

	// Search for the first attached transmitter and turn it on
	//

	for (short id = 0; id < ATC3DG.m_config.numberTransmitters; id++)
	{
		if ((pXmtr + id)->m_config.attached)
		{
			// Transmitter selection is a system function.
			// Using the SELECT_TRANSMITTER parameter we send the id of the
			// transmitter that we want to run with the SetSystemParameter() call
			errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
			if (errorCode != BIRD_ERROR_SUCCESS)
			{
				errorHandler(errorCode);
				m_lock.unlock();
				return 0;
			}
			break;
		}
	}

	printf("Seting Sensor Parameter\n");
	DATA_FORMAT_TYPE format = DOUBLE_POSITION_QUATERNION;

	// scan the sensors and request a record
	for (int sensorID = 0; sensorID < ATC3DG.m_config.numberSensors; sensorID++)
	{
		errorCode = SetSensorParameter(
			sensorID,			// index number of target sensor
			DATA_FORMAT,		// command parameter type
			&format,			// address of data source buffer
			sizeof(format)		// size of source buffer
			);

		if (errorCode != BIRD_ERROR_SUCCESS) errorHandler(errorCode);
	}

	m_connected = true;

	if (m_connected) printf("[EM]:Connected\n");

	m_lock.unlock();

	return 0;
}
bool mitk::MicroBirdTrackingDevice::OpenConnection()
{
  /* Check whether in setup mode */
  if (this->GetState() != Setup)
  {
    this->SetErrorMessage("Can only try to open the connection if in setup mode");
    return false;
  }

  int errorCode; // Holds error code

  /* Initialize the PCIBIRD driver and DLL */
  errorCode = InitializeBIRDSystem(); // this function can take a few seconds
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    return false;
  }
  /// @todo : check for transmitter serial numbers here?
  // Serial numbers could be compared to known ones for some simple
  //    parameters sanity check (measurement frequency etc.)

  /* Get system configuration */
  errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    return false;
  }

  /* use metric measurements in mm */
  errorCode = SetSystemParameter(METRIC, &m_metric, sizeof(m_metric)); 
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    return false;
  }

  /* Set the measurement rate to m_measurementRate */
  if ((m_measurementRate > 30) && (m_measurementRate < 80))
  {
    errorCode = SetSystemParameter(MEASUREMENT_RATE, &m_measurementRate, sizeof(m_measurementRate)); 
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
      return false;
    }
  }

  /* Set power line frequency */
  if ((m_pl >= 50) && (m_pl <= 60))
  {
    errorCode = SetSystemParameter(POWER_LINE_FREQUENCY, &m_pl, sizeof(m_pl));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
      return false;
    }
  }

  /* Set AGC mode */
  m_agc = m_agcModeBoth ? TRANSMITTER_AND_SENSOR_AGC : SENSOR_AGC_ONLY;
  errorCode = SetSystemParameter(AGC_MODE, &m_agc, sizeof(m_agc));
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    return false;
  }

  /* Get system configuration */
  errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    return false;
  }

  /* Get sensor information */
  m_SensorConfig = new SENSOR_CONFIGURATION[m_SystemConfig.numberSensors];
  for (int i = 0; i < m_SystemConfig.numberSensors; i++)
  {
    errorCode = GetSensorConfiguration(i, &(m_SensorConfig[i]));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
    }

    /* Initialize the quality parameter structure */
    QUALITY_PARAMETERS qualityParameters; // = { 164, 0, 32, 3072 };
    GetSensorParameter(i, QUALITY, &qualityParameters, sizeof(qualityParameters));

    /* Set data format to matrix format */
    //DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_MATRIX_TIME_Q;
    /* Set data format to quaternion format */
    DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_QUATERNION_TIME_Q;

    /* Set data format for sensor */
    DATA_FORMAT_TYPE *pTempBuffer = &tempBuffer;
    errorCode = SetSensorParameter(i, DATA_FORMAT, pTempBuffer, sizeof(tempBuffer));  
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
    }
  }

  /* Initialize tools vector */
  {
    MutexLockHolder(*m_ToolsMutex);
    for (int i = 0; i < m_SystemConfig.numberSensors; i++)
    {
      if (m_SensorConfig[i].attached)
        m_Tools.push_back(ToolType::New());

    }
  }

  /* Get transmitter configuration */
  m_TransmitterConfig = new TRANSMITTER_CONFIGURATION[m_SystemConfig.numberTransmitters];
  for (int i = 0; i < m_SystemConfig.numberTransmitters; i++)
  {
    errorCode = GetTransmitterConfiguration(i, &(m_TransmitterConfig[i]));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
    }
  }

  /* Switch off transmitter */
  SwitchTransmitter(true);
  SwitchTransmitter(false);

  // @todo : set up error scaling?

  /* finish  - now all tools should be added, initialized and enabled, so that tracking can be started */
  this->SetState(Ready);
  this->SetErrorMessage("");
  return true; // Return success
}
int main2()
{
	//Tracker			tracker(10000);
	CSystem			PCIBird;
	CSensor			*pSensor;
	CXmtr			*pXmtr;
	int				errorCode;
	int				i;
	int				sensorID;
	short			id;
	int				records = 10000;
	double			rate = 255.0f;

	printf("\n\n");
	printf("ATC3DG Timing Test Application\n");

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// Initialize the PCIBIRD driver and DLL
	//
	// It is always necessary to first initialize the PCIBird "system". By
	// "system" we mean the set of PCIBird cards installed in the PC. All cards
	// will be initialized by a single call to InitializeBIRDSystem(). This
	// call will first invoke a hardware reset of each board. If at any time 
	// during operation of the system an unrecoverable error occurs then the 
	// first course of action should be to attempt to Recall InitializeBIRDSystem()
	// if this doesn't restore normal operating conditions there is probably a
	// permanent failure - contact tech support.
	// A call to InitializeBIRDSystem() does not return any information.
	//
	printf("Initializing ATC3DG system...\n");
	errorCode = InitializeBIRDSystem();
	if(errorCode!=BIRD_ERROR_SUCCESS) 
	{
		errorHandler(errorCode);
		exit(1);
	}

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// GET SYSTEM CONFIGURATION
	//
	// In order to get information about the system we have to make a call to
	// GetBIRDSystemConfiguration(). This call will fill a fixed size structure
	// containing amongst other things the number of boards detected and the
	// number of sensors and transmitters the system can support (Note: This
	// does not mean that all sensors and transmitters that can be supported
	// are physically attached)
	//
	errorCode = GetBIRDSystemConfiguration(&PCIBird.m_config);
	if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// GET SENSOR CONFIGURATION
	//
	// Having determined how many sensors can be supported we can dynamically
	// allocate storage for the information about each sensor.
	// This information is acquired through a call to GetSensorConfiguration()
	// This call will fill a fixed size structure containing amongst other things
	// a status which indicates whether a physical sensor is attached to this
	// sensor port or not.
	//
	pSensor = new CSensor[PCIBird.m_config.numberSensors];
	for(i=0;i<PCIBird.m_config.numberSensors;i++)
	{
		errorCode = GetSensorConfiguration(i, &(pSensor+i)->m_config);
		if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
	}

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// GET TRANSMITTER CONFIGURATION
	//
	// The call to GetTransmitterConfiguration() performs a similar task to the 
	// GetSensorConfiguration() call. It also returns a status in the filled
	// structure which indicates whether a transmitter is attached to this
	// port or not. In a single transmitter system it is only necessary to 
	// find where that transmitter is in order to turn it on and use it.
	//
	pXmtr = new CXmtr[PCIBird.m_config.numberTransmitters];
	for(i=0;i<PCIBird.m_config.numberTransmitters;i++)
	{
		errorCode = GetTransmitterConfiguration(i, &(pXmtr+i)->m_config);
		if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
	}

	//
	// Set the measurement rate
	//
	errorCode = SetSystemParameter(MEASUREMENT_RATE, &rate, sizeof(rate));
	if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);

	//
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// Search for the first attached transmitter and turn it on
	//
	for(id=0;id<PCIBird.m_config.numberTransmitters;id++)
	{
		if((pXmtr+id)->m_config.attached)
		{
			// Transmitter selection is a system function.
			// Using the SELECT_TRANSMITTER parameter we send the id of the
			// transmitter that we want to run with the SetSystemParameter() call
			errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
			if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
			break;
		}
	}

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// Collect data from all birds
	// Loop through all sensors and get a data record if the sensor is attached.
	// Print result to screen
	// Note: The default data format is DOUBLE_POSITION_ANGLES. We can use this
	// format without first setting it.
	// 
	//
	DOUBLE_POSITION_ANGLES_TIME_Q_RECORD record[8*4], *pRecord = record;

	// Set the data format type for each attached sensor.
	for(i=0;i<PCIBird.m_config.numberSensors;i++)
	{
		DATA_FORMAT_TYPE type = DOUBLE_POSITION_ANGLES_TIME_Q;
		errorCode = SetSensorParameter(i,DATA_FORMAT,&type,sizeof(type));
		if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
	}

	// collect as many records as specified in the records var
	printf("Collecting %d data records at %3.2fhz...\n", records, rate*3.0);
	for(i=0;i<records;i++)
	{
		errorCode = GetSynchronousRecord(ALL_SENSORS, pRecord, sizeof(record[0]) * PCIBird.m_config.numberSensors);
		if(errorCode!=BIRD_ERROR_SUCCESS) 
		{
			errorHandler(errorCode);
		}

		// Get a snap shot of the PC's time
		time_t currentTime;
		time(&currentTime);

		// Some user IO, every 500 records print a progress indication
		if( !(i % 500)) 
			printf( ".");

		// scan the sensors and request a record if the sensor is physically attached
		for(sensorID=0;sensorID<PCIBird.m_config.numberSensors;sensorID++)
		{
			// get the status of the last data record
			// only report the data if everything is okay
			unsigned int status = GetSensorStatus( sensorID);

			if( status == VALID_STATUS)
			{
				// save output to buffer
				sprintf(output[i][sensorID], "[%d] 0x%04x %8.3f %8.3f %8.3f: %8.2f %8.2f %8.2f %8.4f\n",
					sensorID,
					status,
					record[sensorID].x,
					record[sensorID].y,
					record[sensorID].z,
					record[sensorID].a,
					record[sensorID].e,
					record[sensorID].r,
					record[sensorID].time
					);
			}  
		}
	}

	printf("\nSaving results to 'GetSynchronousRecord.dat'..\n");

	FILE *fp = fopen( "GetSynchronousRecord.dat", "w");
	for(i=0;i<records;i++)
		for(sensorID=0;sensorID<PCIBird.m_config.numberSensors;sensorID++)
			fprintf( fp, output[i][sensorID]);
	fclose( fp);
	printf("Complete...\n");

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	// Turn off the transmitter before exiting
	// We turn off the transmitter by "selecting" a transmitter with an id of "-1"
	//
	id = -1;
	errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
	if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);

	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	//
	//  Free memory allocations before exiting
	//
	delete[] pSensor;
	delete[] pXmtr;

	return 0;
}