예제 #1
0
//==============================================================================
int COptoTrack::ActivateSensors(  ){
  int iErr;

  if( !IsRunning()) {
    CP_printf("Cannot activate sensors. Tracker is not active!\n");
    return 1;
  }
  iErr = OptotrakActivateMarkers();
  if( iErr ) {
    CP_printf("Error: OptotrakActivateMarkers() failed!\n");
    return 1;
  }

  return 0;
}
예제 #2
0
void main( int argc, unsigned char *argv[] )
{
	OptotrakSettings
		dtSettings;
    int
		nCurDevice,
		nCurMarker,
		nDevices,
		nMarkersToActivate;
	ApplicationDeviceInformation
		*pdtDevices;
    DeviceHandle
        *pdtDeviceHandles;
    DeviceHandleInfo
        *pdtDeviceHandlesInfo;
	unsigned int
		uFlags,
		uElements,
        uChannelCnt,
		uFrameNumber;
    Position3d
        *p3dData;
    static int
        puOdauData[ SAMPLE_NUM_CHANNELS + 1];
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];


    /*
     * initialization
	 * intialize variables
     */
	pdtDevices = NULL;
	pdtDeviceHandles = NULL;
	pdtDeviceHandlesInfo = NULL;
	p3dData = NULL;
	dtSettings.nMarkers = 0;
	dtSettings.fFrameFrequency = SAMPLE_FRAMEFREQ;
	dtSettings.fMarkerFrequency = SAMPLE_MARKERFREQ;
	dtSettings.nThreshold = 30;
	dtSettings.nMinimumGain = 160;
	dtSettings.nStreamData = SAMPLE_STREAMDATA;
	dtSettings.fDutyCycle = SAMPLE_DUTYCYCLE;
	dtSettings.fVoltage = SAMPLE_VOLTAGE;
	dtSettings.fCollectionTime = 1.0;
	dtSettings.fPreTriggerTime = 0.0;
	nDevices = 0;
	nMarkersToActivate = 0;

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak Certus sample program #19\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Communication Initialization
     * Once the system processors have been loaded, the application
     * prepares for communication by initializing the system processors.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * Determine if this sample will run with the system attached.
	 * This sample is intended for Optotrak Certus systems.
	 */
	fprintf( stdout, "...DetermineSystem\n" );
	if( uDetermineSystem( ) != OPTOTRAK_CERTUS_FLAG )
	{
		goto ERROR_EXIT;
	} /* if */

    /*
     * Strober Initialization
     * Once communication has been initialized, the application must
     * determine the strober configuration.
     * The application retrieves device handles and all strober
     * properties from the system.
     */
	fprintf( stdout, "...DetermineStroberConfiguration\n" );
	if( DetermineStroberConfiguration( &pdtDeviceHandles, &pdtDeviceHandlesInfo, &nDevices ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * check if any devices have been detected by the system
	 */
	if( nDevices == 0 )
	{
		fprintf( stdout, ".........no devices detected.  Quitting program...\n" );
		goto PROGRAM_COMPLETE;
	} /* if */

    /*
     * Now that all the device handles have been completely set up,
     * the application can store all the device handle information in
     * an internal data structure.  This will facilitate lookups when
     * a property setting needs to be checked.
     */
    ApplicationStoreDeviceProperties( &pdtDevices, pdtDeviceHandlesInfo, nDevices );

	/*
	 * Change the number of markers to fire for all devices
	 */
	for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
	{
		nMarkersToActivate = pdtDevices[nCurDevice].b3020Capability?
							   CERTUS_SAMPLE_3020_STROBER_MARKERSTOFIRE : CERTUS_SAMPLE_STROBER_MARKERSTOFIRE;

		SetMarkersToActivateForDevice( &(pdtDevices[nCurDevice]), pdtDeviceHandlesInfo[nCurDevice].pdtHandle->nID, nMarkersToActivate );
	} /* if */
	fprintf( stdout, "\n" );

	/*
	 * Determine the collection settings based on the device properties
	 */
	ApplicationDetermineCollectionParameters( nDevices, pdtDevices, &dtSettings );


    /*
     * Set optional processing flags (this overides the settings in Optotrak.INI).
     */
	fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
    if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
                                    OPTO_CONVERT_ON_HOST |
                                    OPTO_RIGID_ON_HOST ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
	 * Load camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * Ensure that we are firing some markers
	 */
	if( dtSettings.nMarkers == 0 )
	{
		fprintf( stdout, "Error: There are no markers to be activated.\n" );
		goto ERROR_EXIT;
	} /* if */

	/*
	 * allocate memory for 3d data
	 */
	p3dData = (Position3d*)malloc( dtSettings.nMarkers * sizeof( Position3d ) );

    /*
     * Configure ODAU Collection.
     */
	fprintf( stdout, "...OdauSetupCollection\n" );
    if( OdauSetupCollection( ODAU1,
							 SAMPLE_NUM_CHANNELS,		/* Number of analog channels to collect. */
							 SAMPLE_ODAU_GAIN,			/* Gain to use for the analog channels.*/
							 ODAU_DIGITAL_INPB_INPA,	/* Mode for the Digital I/O port.*/
							 (float)100.0,				/* Frequency to collect data frames at. */
							 (float)90000.0,			/* Frequency to scan channels at. */
							 SAMPLE_STREAMDATA,			/* Stream mode for the data buffers. */
							 (float)1.0,				/* Number of seconds of data to collect. */
							 0.0,						/* Number of seconds to pre-trigger data by. */
							 0 ) )						/* Flags. */
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Configure Optotrak Collection
     * Once the system strobers have been enabled, and all settings are
     * loaded, the application can set up the Optotrak collection
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
	fprintf( stdout, ".....%d, %.2f, %.0f, %d, %d, %d, %.2f, %.2f, %.0f, %.0f\n",
								 dtSettings.nMarkers,
			                     dtSettings.fFrameFrequency,
				                 dtSettings.fMarkerFrequency,
					             dtSettings.nThreshold,
						         dtSettings.nMinimumGain,
							     dtSettings.nStreamData,
								 dtSettings.fDutyCycle,
								 dtSettings.fVoltage,
								 dtSettings.fCollectionTime,
								 dtSettings.fPreTriggerTime );
    if( OptotrakSetupCollection( dtSettings.nMarkers,
			                     dtSettings.fFrameFrequency,
				                 dtSettings.fMarkerFrequency,
					             dtSettings.nThreshold,
						         dtSettings.nMinimumGain,
							     dtSettings.nStreamData,
								 dtSettings.fDutyCycle,
								 dtSettings.fVoltage,
								 dtSettings.fCollectionTime,
								 dtSettings.fPreTriggerTime,
								 OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG | OPTOTRAK_SWITCH_AND_CONFIG_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

	/*
	 * Activate markers.
	 */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
	if( OptotrakActivateMarkers( ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */
	sleep( 1 );

	/*
	 * loop around until a device is plugged or unplugged
	 */
	while( 1 )
	{
		fprintf( stdout, "\nCHANGE THE STROBER CONFIGURATION OR PRESS SWITCH TO END THIS TEST\n\n" );
		sleep( 1 );

		if( DataGetLatest3D( &uFrameNumber, &uElements, &uFlags, p3dData ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

        fprintf( stdout, "\nFrame Number: %8u\n", uFrameNumber );

		/*
		 * display the 3d data
		 */
		fprintf( stdout, "3D Data Display (%d Markers)\n", dtSettings.nMarkers );
		for( nCurMarker = 0; nCurMarker < dtSettings.nMarkers; nCurMarker++ )
		{
			DisplayMarker( nCurMarker + 1, p3dData[nCurMarker] );
		} /* for */

		/*
		 * check if the tool configuration has changed
		 */
		if( uFlags & OPTO_TOOL_CONFIG_CHANGED_FLAG )
		{
			/*
			 * exit the program loop
			 */
			fprintf( stdout, "\n\n\n...Tool Configuration Changed\n" );
			break;
		} /* if */

		if( uFlags & OPTO_SWITCH_DATA_CHANGED_FLAG )
		{
			/*
			 * exit the program loop
			 */
			fprintf( stdout, "\n\n\n...Switch Data Changed\n" );
			break;
		} /* if */

        /*
         * Get a frame of ODAU raw data.
         */
        fprintf( stdout, "\n" );
        if( DataGetLatestOdauRaw( ODAU1, &uFrameNumber, &uElements, &uFlags, puOdauData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the data.
         */
        for( uChannelCnt = 0; uChannelCnt < SAMPLE_NUM_CHANNELS; ++uChannelCnt )
        {
            fprintf( stdout, "Channel %u  Raw 0x%08x  Voltage %f\n",
                     uChannelCnt + 1,
                     puOdauData[ uChannelCnt],
                     (float)( (int)( puOdauData[uChannelCnt])) * 0.000305175/(float)SAMPLE_ODAU_GAIN );
            /*
             * if you have an ODAUI unit, instead of an ODAUII unit,
             * replace the above fprintf statement with:
             * fprintf( stdout, "Channel %u  Raw 0x%08x  Voltage %f\n",
             *          uChannelCnt + 1,
             *          puOdauData[ uChannelCnt ],
             *          (float)( (int)( puOdauData[ uChannelCnt] & 0x0FFF ) - 2048) *
             *                          (10.0/2048.0)/(float)SAMPLE_ODAU_GAIN );
             */
        } /* for */
        fprintf( stdout, "Digital 0x%04x\n", puOdauData[SAMPLE_NUM_CHANNELS] );

	} /* while */

	sleep( 1 );


PROGRAM_COMPLETE:
    /*
     * CLEANUP
     */
	fprintf( stdout, "\n" );
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers( );
    TransputerShutdownSystem( );

	/*
	 * free all memory
	 */
	if( pdtDeviceHandlesInfo )
	{
		for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
		{
			AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[nCurDevice].grProperties), 0 );
		} /* for */
	} /* if */
	AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
	AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );

	exit( 0 );


ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString, MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
	OptotrakDeActivateMarkers( );
	TransputerShutdownSystem( );

	/*
	 * free all memory
	 */
	if( pdtDeviceHandlesInfo )
	{
		for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
		{
			AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[nCurDevice].grProperties), 0 );
		} /* for */
	} /* if */
	AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
	AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );

    exit( 1 );

} /* main */
예제 #3
0
파일: poapi.c 프로젝트: Fperdreau/Poapi
int main(){
	// Function to retrieve OAPI version
	size_t len = LEN;
	size_t read;
	char *line = malloc(len);
	assert(line);
	// return variables
	char s0[1024];
	int i0;
	int nFrameOld = -1;

	
	while ((read = getline(&line, &len, stdin)) != -1) {
		//fprintf(stderr, "INFO: Retrieved line of length %zu (including NULL): %s", read, line);
		if (cmp(line, "OAPIGetVersionString")){
			i0 = OAPIGetVersionString(s0, len);
			printf("'%s'\n", s0);
		} else if (cmp(line, "OptoGetLastError")){
			i0 = OptoGetLastError(s0, len);
			printf("'%s'\n", s0);
		} else if (cmp(line, "OptotrakGetErrorString")){
			i0 = OptotrakGetErrorString(s0, len);
			printf("'%s'\n", s0);
		} else if (cmp(line, "OptoGetExtendedErrorCode")){
			i0 = OptoGetExtendedErrorCode();
			printf("%d\n", i0);
		} else if (cmp(line, "TransputerLoadSystem")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\""); // may be null
			i0 = TransputerLoadSystem(s);
			//i0 = TransputerLoadSystem("/opt/NDIoapi/ndigital/realtime/system.nif");
			printf("%d\n", i0);
		} else if (cmp(line, "TransputerInitializeSystem")){
			i0 = TransputerInitializeSystem(OPTO_LOG_ERRORS_FLAG);
			printf("%d\n", i0);
		} else if (cmp(line, "TransputerShutdownSystem")){
			i0 = TransputerShutdownSystem();
			printf("%d\n", i0);
		} else if (cmp(line, "TransputerDetermineSystemCfg")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\""); // may be null
			i0 = TransputerDetermineSystemCfg(s);
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakLoadCameraParameters")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\""); // may be null
			OptotrakLoadCameraParameters(s);
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakLoadAutoScale")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\""); // may be null
			OptotrakLoadAutoScale(s);
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakLockTemperatures")){
			i0 = OptotrakLockTemperatures();
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakSetCollectionFile")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\""); // may be null
			OptotrakSetCollectionFile(s);
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakSetupCollectionFromFile")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\""); // may be null
			OptotrakSetupCollectionFromFile(s);
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakSetupCollection")){
			float fFrameFrequency, fMarkerFrequency;
			int nThreshold, nMinimumGain, nStreamData;
			float fDutyCycle, fVoltage, fCollectionTime, fPreTriggerTime;
			int nFlags;
			
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			int i = sscanf(s, "%d, %f, %f, %d, %d, %d, %f, %f, %f, %f, %d", 
				&nMarkers, 
				&fFrameFrequency, &fMarkerFrequency,
				&nThreshold, &nMinimumGain, &nStreamData,
				&fDutyCycle, &fVoltage, &fCollectionTime, &fPreTriggerTime,
				&nFlags);
			if(i==11){
				i0 = OptotrakSetupCollection(nMarkers, 
					fFrameFrequency, fMarkerFrequency, 
					nThreshold, nMinimumGain, nStreamData,
					fDutyCycle, fVoltage, fCollectionTime, fPreTriggerTime, 
					nFlags);
				printf("%d\n", i0);
				
				allocSpace(nMarkers);
			} else
				printf("ERROR: %d arguments read (11 expected)\n", i);
		} else if (cmp(line, "OptotrakActivateMarkers")){
			i0 = OptotrakActivateMarkers();
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakDeActivateMarkers")){
			i0 = OptotrakDeActivateMarkers();
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakGetStatus")){
			int nNumSensors, nNumOdaus, nNumRigidBodies, nMarkers;
			float fFrameFrequency, fMarkerFrequency;
			int nThreshold, nMinimumGain, nStreamData;
			float fDutyCycle, fVoltage, fCollectionTime, fPreTriggerTime;
			int nFlags;
			i0 = OptotrakGetStatus(&nNumSensors, &nNumOdaus, &nNumRigidBodies, &nMarkers,
				&fFrameFrequency, &fMarkerFrequency,
				&nThreshold, &nMinimumGain, &nStreamData, 
				&fDutyCycle, &fVoltage, &fCollectionTime, &fPreTriggerTime,
				&nFlags);
			printf("(%d, %d, %d, %d, %f, %f, %d, %d, %d, %f, %f, %f, %f, 0x%x)\n", 
				nNumSensors, nNumOdaus, nNumRigidBodies, nMarkers,
				fFrameFrequency, fMarkerFrequency,
				nThreshold, nMinimumGain, nStreamData, 
				fDutyCycle, fVoltage, fCollectionTime, fPreTriggerTime,
				nFlags);
		} else if (cmp(line, "OptotrakSetStroberPortTable")){
			int nPort1, nPort2, nPort3, nPort4;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %d, %d, %d", &nPort1, &nPort2, &nPort3, &nPort4) == 4){
				i0 = OptotrakSetStroberPortTable(nPort1, nPort2, nPort3, nPort4);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading 4 arguments\n");
		} else if (cmp(line, "OptotrakSaveCollectionToFile")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\"");
			if (s)
				i0 = OptotrakSaveCollectionToFile(s);
			else
				i0 = OptotrakSaveCollectionToFile("");
			printf("%d\n", i0);
		} else if (cmp(line, "OptotrakSetCameraParameters")){
			int nMarkerType, nWaveLength, nModelType;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %d, %d", &nMarkerType, &nWaveLength, &nModelType) == 3){
				i0 = OptotrakSetCameraParameters(nMarkerType, nWaveLength, nModelType);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading 3 arguments\n");
		} else if (cmp(line, "OptotrakGetCameraParameterStatus")){
			int curMarkerType, nCurWaveLength, nCurModelType;
			char szStatus[len];
			i0 = OptotrakGetCameraParameterStatus(&curMarkerType, &nCurWaveLength, &nCurModelType, szStatus, len);
			printf("(%d, %d, %d, '%s')\n", curMarkerType, nCurWaveLength, nCurModelType, szStatus);
		} else if (cmp(line, "OdauSaveCollectionToFile")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\"");
			if (s){
				i0 = OptotrakSaveCollectionToFile(s);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading filename\n");
		} else if (cmp(line, "OdauSetupCollectionFromFile")){
			strtok(line, "\"");
			char* s = strtok(NULL, "\"");
			if (s){
				i0 = OdauSetupCollectionFromFile(s);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading filename\n");
		} else if (cmp(line, "OdauSetTimer")){
			//int OdauSetTimer( int nOdauId, unsigned uTimer, unsigned uMode, unsigned long ulVal );
			int nOdauId;
			unsigned uTimer, uMode;
			unsigned long ulVal;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %u, %u, %lu", &nOdauId, &uTimer, &uMode, &ulVal) == 4){
				i0 = OdauSetTimer(ODAU1+nOdauId-1, uMode, uTimer, ulVal);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading 4 arguments\n");
		} else if (cmp(line, "OdauSetAnalogOutputs")){
			//int OdauSetAnalogOutputs( int nOdauId, float *pfVoltage1, float *pfVoltage2, unsigned uChangeMask );
			int nOdauId; // 1-4 -> ODAU1-ODAU4
			float fVoltage1;
			float fVoltage2;
			unsigned uChangeMask;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %f, %f, %u", &nOdauId, &fVoltage1, &fVoltage2, &uChangeMask) == 4){
				i0 = OdauSetAnalogOutputs(ODAU1+nOdauId-1, &fVoltage1, &fVoltage2, uChangeMask);
				printf("(%f, %f)\n", fVoltage1, fVoltage1);
			} else
				printf("ERROR: reading 4 arguments\n");
		} else if (cmp(line, "OdauSetDigitalOutputs")){
			//int OdauSetDigitalOutputs( int nOdauId, unsigned *puDigitalOut, unsigned uUpdateMask );
			int nOdauId; // 1-4 -> ODAU1-ODAU4
			unsigned uDigitalOut, uUpdateMask;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %u, %u", &nOdauId, &uDigitalOut, &uUpdateMask) == 4){
				i0 = OdauSetDigitalOutputs(ODAU1+nOdauId-1, &uDigitalOut, uUpdateMask);
				printf("0x%x\n", uDigitalOut);
			} else
				printf("ERROR: reading 3 arguments\n");
		} else if (cmp(line, "OdauSetupCollection")){
			//OdauSetupCollection( int nOdauId, int nChannels, int nGain, int nDigitalMode, float fFrameFreq, float fScanFreq, int nStreamData, float fCollectionTime, float fPreTriggerTime, unsigned uFlags );
			int nOdauId, nChannels, nGain, nDigitalMode;
			float fFrameFreq, fScanFreq;
			int nStreamData;
			float fCollectionTime, fPreTriggerTime;
			unsigned uFlags;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %d, %d, %d, %f, %f, %d, %f, %f, %u", 
					&nOdauId, &nChannels, &nGain, &nDigitalMode, 
					&fFrameFreq, &fScanFreq, &nStreamData,
					&fCollectionTime, &fPreTriggerTime, &uFlags) == 10){
				i0 = OdauSetupCollection(ODAU1+nOdauId-1, nChannels, nGain, nDigitalMode, 
					fFrameFreq, fScanFreq, nStreamData,
					fCollectionTime, fPreTriggerTime, uFlags);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading 10 arguments\n");
		} else if (cmp(line, "OptotrakStopCollection")){
			i0 = OptotrakStopCollection();
			printf("%d\n", i0);
		} else if (cmp(line, "OdauGetStatus")){
			 //int OdauGetStatus( int nOdauId, int *pnChannels, int *pnGain, int *pnDigitalMode, float *pfFrameFrequency, float *pfScanFrequency, int *pnStreamData, float *pfCollectionTime, float *pfPreTriggerTime, unsigned  *puCollFlags, int *pnFlags );
			int nOdauId, nChannels, nGain, nDigitalMode;
			float fFrameFrequency, fScanFrequency;
			int nStreamData;
			float fCollectionTime, fPreTriggerTime;
			unsigned  uCollFlags;
			int nFlags;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d", &nOdauId) == 1){
				i0 = OdauGetStatus(ODAU1+nOdauId-1, &nChannels, &nGain, &nDigitalMode, 
					&fFrameFrequency, &fScanFrequency, &nStreamData, &fCollectionTime, &fPreTriggerTime, 
					&uCollFlags, &nFlags);
				printf("(%d, %d, %d, %f, %f, %d, %f, %f, 0x%04x, %d)\n", 
					nChannels, nGain, nDigitalMode, 
					fFrameFrequency, fScanFrequency, nStreamData, fCollectionTime, fPreTriggerTime, 
					uCollFlags, nFlags);
			} else
				printf("ERROR: reading argument\n");
		} else if (cmp(line, "RigidBodyAddFromFile")){
			int nRigidBodyId, nStartMarker;
			char szRigFile[len];
			int nFlags;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %d, %s, %d", &nRigidBodyId, &nStartMarker, szRigFile, &nFlags) != 4){
				i0 = RigidBodyAddFromFile(nRigidBodyId, nStartMarker, szRigFile, nFlags);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading 4 arguments\n");
		} else if (cmp(line, "RigidBodyAdd")){
			//int ( int nRigidBodyId, int nStartMarker, int nNumMarkers, float *pRigidCoordinates, float *pNormalCoordinates, int nFlags );
			int nRigidBodyId, nStartMarker, nNumMarkers;
			float *rigidCoordinates, *normalCoordinates;
			int nFlags;
			char* s = strtok(line, "(");
			
			if(s && sscanf(line+strlen(s)+1, "%d, %d, %d", &nRigidBodyId, &nStartMarker, &nNumMarkers)==3){
				rigidCoordinates = calloc(3*nNumMarkers, sizeof(float));
				normalCoordinates = calloc(3*nNumMarkers, sizeof(float));
				s = strtok(NULL, "[("); // up to first list/tuple
				if(!s){
					printf("ERROR: reading rigid values\n");
					goto end;
				}
				for(int i=0; i<3*nNumMarkers; i++){
					char* s = strtok(NULL, ",");
					if(!s){
						printf("ERROR: reading rigid value %d\n", i);
						goto end;
					}
					if(sscanf(s, "%f", (float*)&rigidCoordinates[i]) != 1){
						printf("ERROR: parsing rigid value %d\n", i);
						goto end;
					}
				}

				s = strtok(NULL, "[("); // up to first list/tuple
				if(!s){
					printf("ERROR: reading normal values\n");
					goto end;
				}
				for(int i=0; i<3*nNumMarkers; i++){
					char* s = strtok(NULL, ",");
					if(!s){
						printf("ERROR: reading normal value %d\n", i);
						goto end;
					}
					if(sscanf(s, "%f", (float*)&normalCoordinates[i]) != 1){
						printf("ERROR: parsing normal value %d\n", i);
						goto end;
					}
				}
				s = strtok(NULL, "],) ");
				//s = strtok(NULL, ")");
				if(s && sscanf(s, "%d", &nFlags)==1){
					i0 = RigidBodyAdd(nRigidBodyId, nStartMarker, nNumMarkers, rigidCoordinates, normalCoordinates, nFlags);
					printf("%d\n", i0);
				} else
					printf("ERROR: reading last argument\n");
			} else
				printf("ERROR: reading first 3 arguments\n");
			//int RigidBodyAddFromFile( int nRigidBodyId, int nStartMarker, char *pszRigFile, int nFlags );
			//int RigidBodyChangeSettings( int nRigidBodyId, int nMinMarkers, int nMaxMarkersAngle, float fMax3dError, float fMaxSensorError, float fMax3dRmsError, float fMaxSensorRmsError, int nFlags );
			//int RigidBodyDelete( int nRigidBodyId );
			//int RigidBodyChangeFOR( int nRigidId, int nRotationMethod );
		} else if (cmp(line, "RigidBodyChangeSettings")){
			int nRigidBodyId, nMinMarkers, nMaxMarkersAngle;
			float fMax3dError, fMaxSensorError, fMax3dRmsError, fMaxSensorRmsError;
			int nFlags;
			strtok(line, "(");
			char* s = strtok(NULL, ")");
			if(s && sscanf(s, "%d, %d, %d, %f, %f, %f, %f, %d", 
				&nRigidBodyId, &nMinMarkers, &nMaxMarkersAngle, 
				&fMax3dError, &fMaxSensorError, &fMax3dRmsError, &fMaxSensorRmsError, &nFlags) != 4){
				i0 = RigidBodyChangeSettings(nRigidBodyId, nMinMarkers, nMaxMarkersAngle,
				fMax3dError, fMaxSensorError, fMax3dRmsError, fMaxSensorRmsError, nFlags);
				printf("%d\n", i0);
			} else
				printf("ERROR: reading 8 arguments\n");
		} else if (cmp(line, "DataGetLatestRaw")){
			uint uFrameNumber=999, uElements=999, uFlags=999;
			i0 = DataGetLatestRaw(&uFrameNumber, &uElements, &uFlags, pFullRawData);
			printf("Frame Number: %8u\n", uFrameNumber);
			printf("Elements    : %8u\n", uElements);
			printf("Flags       :     0x%04x\n", uFlags);
			for(uint uMarkerCnt=0; uMarkerCnt<nMarkers; uMarkerCnt++){
				// Print out the current marker number.
				fprintf( stdout, "Marker %u, ", uMarkerCnt + 1 );
				//Print out the information for each sensor.
				for(uint uSensorCnt = 0; uSensorCnt<NUM_SENSORS; ++uSensorCnt ){
					//Print out the current sensor number.
					fprintf( stdout, "Sensor %u, ", uSensorCnt + 1 );
					// Print out the centroid. If it is bad print out *the string	'missing'.
					//if( pFullRawData[uMarkerCnt].fCentroid[uSensorCnt] < MAX_NEGATIVE )
						//printf("data missing, " );
					//else 
						printf("data: %8.2g, ", (float)pFullRawData[uMarkerCnt].fCentroid[uSensorCnt] );
					//Print out the rest of this sensor’s information.
					printf("peak: %4d, DRC: %4d, sensorCode: %u\n",
						pFullRawData[uMarkerCnt].SensorData[uSensorCnt].ucPeak,
						pFullRawData[uMarkerCnt].SensorData[uSensorCnt].ucDRC,
						(uint)pFullRawData[uMarkerCnt].SensorData[uSensorCnt].ucCode);
				} /* for */
			} /* for */
		} else if (cmp(line, "DataGetLatest3D")){
			//int DataGetLatest3D( unsigned *pnFrame, unsigned *pElems, unsigned *pFlags, void *pDataDest );
			uint nFrame=999, elems=999, flags=999;
			//Position3d dataDest[NUM_MARKERS];
			i0 = DataGetLatest3D(&nFrame, &elems, &flags, pPosition3d);
			printf("(%d, %u, %u, 0x%X, (", i0, nFrame, elems, flags);
			for(uint i=0; i<nMarkers; i++){
				printf("  (%10.3g, %10.3g, %10.3g),", pPosition3d[i].x, pPosition3d[i].y, pPosition3d[i].z);
			}
			printf("))\n");
		} else if (cmp(line, "RequestLatest3D")){
			i0 = RequestLatest3D();
			printf("%d\n", i0);
		} else if (cmp(line, "DataIsReady")){
			i0 = DataIsReady();
			printf("%d\n", i0);
		} else if (cmp(line, "DataReceiveLatest3D")){
			// alle markers 3d 
			uint nFrame=999, elems=999, flags=999;
			Position3d dataDest;
			i0 = DataReceiveLatest3D(&nFrame, &elems, &flags, &dataDest);
			printf("(%d, %u, %u, 0x%X, (", i0, nFrame, elems, flags);
			for(uint i=0; i<nMarkers; i++){
				printf("  (%10.3g, %10.3g, %10.3g),", pPosition3d[i].x, pPosition3d[i].y, pPosition3d[i].z);
			}
			printf("))\n");
		} else if (cmp(line, "DataReceiveLatestRaw")){
			uint nFrame, elems, flags;
			void *dataDest; // todo allocate!
			i0 = DataReceiveLatestRaw(&nFrame, &elems, &flags, &dataDest);
			printf("(%d, %u, 0x%x)\n", i0, nFrame, flags);
		} else if (cmp(line, "DataBufferInitializeFile")){
			//unsigned uDataId;
			char pszFileName[len];
			strtok(line, "\"");
			char* s = strtok(NULL, "\"");
			if(sscanf(s, "%s", pszFileName)==1){
				i0 = DataBufferInitializeFile(OPTOTRAK, s);
				printf("%d\n", i0);
			} else
				printf("ERROR: could not read filename\n");
		} else if (cmp(line, "DataBufferSpoolData")){
			int i = 999;
			i0 = DataBufferSpoolData(&i);
			printf("(%d, %d)\n", i0, i);
		} else if (cmp(line, "DataBufferStart")){
			i0 = DataBufferStart();
			printf("%d\n", i0);
		} else if (cmp(line, "DataBufferStop")){
			i0 = DataBufferStop();
			printf("%d\n", i0);
		} else if (cmp(line, "DataBufferWriteData")){
			unsigned uRealtimeData=999, uSpoolComplete=0, uSpoolStatus=999;
			unsigned long ulFramesBuffered=999;
			i0 = DataBufferWriteData(&uRealtimeData, &uSpoolComplete, &uSpoolStatus, &ulFramesBuffered);
			printf("(%d, %u, %u, %u, %lu)\n", i0, uRealtimeData, uSpoolComplete, uSpoolStatus, ulFramesBuffered);
		} else if (cmp(line, "DataBufferAbortSpooling")){
			i0 = DataBufferAbortSpooling();
			printf("%d\n", i0);
		} else {
			printf("ERROR: no such command: %s\n", line);
		}
		end:
		fflush(stdout);
	}
	exit(EXIT_SUCCESS);
}
예제 #4
0
void main( int argc, unsigned char *argv[] )
{
    OptotrakSettings
    dtSettings;
    char
    szNDErrorString[MAX_ERROR_STRING_LENGTH + 1],
                    szProperty[32];
    int
    i,
    nCurDevice,
    nCurProperty,
    nCurFrame,
    nCurMarker,
    nMarkersToActivate,
    nDevices,
    nDeviceMarkers;
    ApplicationDeviceInformation
    *pdtDevices;
    DeviceHandle
    *pdtDeviceHandles;
    DeviceHandleInfo
    *pdtDeviceHandlesInfo;
    unsigned int
    uFlags,
    uElements,
    uFrameNumber;
    Position3d
    *p3dData;
    char
    *pChar,
    szInput[10];


    /*
     * initialization
     * intialize variables
     */
    pdtDevices = NULL;
    pdtDeviceHandles = NULL;
    pdtDeviceHandlesInfo = NULL;
    p3dData = NULL;
    nMarkersToActivate = 0;
    nDevices = 0;
    nDeviceMarkers = 0;
    dtSettings.nMarkers = 0;
    dtSettings.fFrameFrequency = SAMPLE_FRAMEFREQ;
    dtSettings.fMarkerFrequency = SAMPLE_MARKERFREQ;
    dtSettings.nThreshold = 30;
    dtSettings.nMinimumGain = 160;
    dtSettings.nStreamData = SAMPLE_STREAMDATA;
    dtSettings.fDutyCycle = SAMPLE_DUTYCYCLE;
    dtSettings.fVoltage = SAMPLE_VOLTAGE;
    dtSettings.fCollectionTime = 1.0;
    dtSettings.fPreTriggerTime = 0.0;

    /*
     * Announce that the program has started
     */
    fprintf( stdout, "\nOptotrak Certus sample program\n\n" );

    /*
     * look for the -nodld parameter that indicates 'no download'
     */
    if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
    {
        /*
         * Load the system of processors.
         */
        fprintf( stdout, "...TransputerLoadSystem\n" );
        if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
        {
            goto ERROR_EXIT;
        } /* if */

        sleep( 1 );
    } /* if */

    /*
     * Communication Initialization
     * Once the system processors have been loaded, the application
     * prepares for communication by initializing the system processors.
     */
    fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Determine if this sample will run with the system attached.
     * This sample is intended for Optotrak Certus systems.
     */
    fprintf( stdout, "...DetermineSystem\n" );
    if( uDetermineSystem( ) != OPTOTRAK_CERTUS_FLAG )
    {
        goto PROGRAM_COMPLETE;
    } /* if */


EXECUTION_START:
    /*
     * Strober Initialization
     * Once communication has been initialized, the application must
     * determine the strober configuration.
     * The application retrieves device handles and all strober
     * properties from the system.
     */
    fprintf( stdout, "...DetermineStroberConfiguration\n" );
    if( DetermineStroberConfiguration( &pdtDeviceHandles, &pdtDeviceHandlesInfo, &nDevices ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Now that all the device handles have been completely set up,
     * the application can store all the device handle information in
     * an internal data structure.  This will facilitate lookups when
     * a property setting needs to be checked.
     */
    ApplicationStoreDeviceProperties( &pdtDevices, pdtDeviceHandlesInfo, nDevices );

    /*
     * Change the number of markers to fire for all devices
     */
    for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
    {
        nMarkersToActivate = pdtDevices[nCurDevice].b3020Capability?
                             CERTUS_SAMPLE_3020_STROBER_MARKERSTOFIRE : CERTUS_SAMPLE_STROBER_MARKERSTOFIRE;

        SetMarkersToActivateForDevice( &(pdtDevices[nCurDevice]), pdtDeviceHandlesInfo[nCurDevice].pdtHandle->nID, nMarkersToActivate );
    } /* if */
    fprintf( stdout, "\n" );

    /*
     * Retrieve the device properties again to verify that the changes took effect.
     */
    for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
    {
        if( GetDevicePropertiesFromSystem( &(pdtDeviceHandlesInfo[nCurDevice]) ) != OPTO_NO_ERROR_CODE )
        {
            goto ERROR_EXIT;
        } /* if */
    } /* for */

    if( ApplicationStoreDeviceProperties( &pdtDevices, pdtDeviceHandlesInfo, nDevices ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Display all the property values
     */
    for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
    {
        fprintf( stdout, "...Device %d properties:\n", nCurDevice );
        for( nCurProperty = 0; nCurProperty < pdtDeviceHandlesInfo[nCurDevice].nProperties; nCurProperty++ )
        {
            /*
             * Determine the property
             */
            switch( pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].uPropertyID )
            {
            case DH_PROPERTY_NAME:
                sprintf( szProperty, "Device Name          " );
                break;

            case DH_PROPERTY_MARKERSTOFIRE:
                sprintf( szProperty, "Markers To Activate  " );
                /*
                 * add to the number of total markers being fired
                 */
                dtSettings.nMarkers += pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.nData;
                break;

            case DH_PROPERTY_MAXMARKERS:
                sprintf( szProperty, "MaxDeviceMarkers     " );
                break;

            case DH_PROPERTY_STARTMARKERPERIOD:
                sprintf( szProperty, "Start Marker Period  " );
                break;

            case DH_PROPERTY_SWITCHES:
                sprintf( szProperty, "Device Switches      " );
                break;

            case DH_PROPERTY_VLEDS:
                sprintf( szProperty, "Device VLEDs         " );
                break;

            case DH_PROPERTY_PORT:
                sprintf( szProperty, "Strober Port         " );
                break;

            case DH_PROPERTY_ORDER:
                sprintf( szProperty, "Strober Port Order   " );
                break;

            case DH_PROPERTY_SUBPORT:
                sprintf( szProperty, "Strober SubPort      " );
                break;

            case DH_PROPERTY_FIRINGSEQUENCE:
                sprintf( szProperty, "Activation Sequence  " );
                break;

            case DH_PROPERTY_HAS_ROM:
                sprintf( szProperty, "Has Rom?             " );
                break;

            case DH_PROPERTY_TOOLPORTS:
                sprintf( szProperty, "Num ToolPorts        " );
                break;

            case DH_PROPERTY_3020_CAPABILITY:
                sprintf( szProperty, "3020 Capability      " );
                break;

            case DH_PROPERTY_3020_MARKERSTOFIRE:
                sprintf( szProperty, "3020 MrkrsToActivate " );
                /*
                 * add to the number of total markers being fired
                 */
                dtSettings.nMarkers += pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.nData;
                break;

            case DH_PROPERTY_3020_STARTMARKERPERIOD:
                sprintf( szProperty, "3020 StartMarkerPrd  " );
                break;

            case DH_PROPERTY_STATUS:
                sprintf( szProperty, "Status               " );
                break;

            case DH_PROPERTY_UNKNOWN:
            default:
                sprintf( szProperty, "Unknown Property %d ", pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].uPropertyID );
                break;

            } /* switch */

            /*
             * Determine the property's data type before displaying it
             */
            switch( pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtPropertyType )
            {
            case DH_PROPERTY_TYPE_INT:
                fprintf( stdout, "......Property_%.2d - %s = %d\n", nCurProperty, szProperty, pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.nData );
                break;
            case DH_PROPERTY_TYPE_FLOAT:
                fprintf( stdout, "......Property_%.2d - %s = %f\n", nCurProperty, szProperty, pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.fData );
                break;
            case DH_PROPERTY_TYPE_DOUBLE:
                fprintf( stdout, "......Property_%.2d - %s = %f\n", nCurProperty, szProperty, pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.dData );
                break;
            case DH_PROPERTY_TYPE_CHAR:
                fprintf( stdout, "......Property_%.2d - %s = %s\n", nCurProperty, szProperty, pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.cData );
                break;
            case DH_PROPERTY_TYPE_STRING:
                if( pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].uPropertyID == DH_PROPERTY_FIRINGSEQUENCE )
                {
                    fprintf( stdout, "......Property_%.2d - %s = ", nCurProperty, szProperty );
                    for( pChar = pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.szData; ( pChar != '\0' ) && ( *pChar != 0 ); pChar++ )
                    {
                        fprintf( stdout, "%.3d, ", *pChar );
                    } /* for */
                    fprintf( stdout, "\n" );
                }
                else
                {
                    fprintf( stdout, "......Property_%.2d - %s = %s\n", nCurProperty, szProperty, pdtDeviceHandlesInfo[nCurDevice].grProperties[nCurProperty].dtData.szData );
                } /* if */
                break;
            default:
                fprintf( stdout, "......Property_%.2d = UNKNOWN PROPERTY TYPE\n", nCurDevice );
                break;
            } /* switch */

        } /* for */

        fprintf( stdout, "\n" );
    } /* for */


    fprintf( stdout, "Continue?  or determine strober configuration Again? [c|a] : " );
    gets( szInput );
    if( szInput[0] == 'a' )
    {
        goto EXECUTION_START;
    } /* if */

    /*
     * Retrieve Optotrak Data
     */

    fprintf( stdout, "\n" );

    /*
     * check if any devices have been detected by the system
     */
    if( nDevices == 0 )
    {
        fprintf( stdout, ".........no devices detected.\n" );
        fprintf( stdout, "Quit program? [y|n] : " );
        gets( szInput );
        if( szInput[0] == 'y' )
        {
            goto PROGRAM_COMPLETE;
        } /* if */

        fprintf( stdout, "...restarting program execution\n\n" );
        goto EXECUTION_START;
    } /* if */

    /*
     * Determine the collection settings based on the device properties
     */
    ApplicationDetermineCollectionParameters( nDevices, pdtDevices, &dtSettings );


    /*
     * Set optional processing flags (this overides the settings in Optotrak.INI).
     */
    fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
    if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
                                    OPTO_CONVERT_ON_HOST |
                                    OPTO_RIGID_ON_HOST ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load camera parameters.
     */
    fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Ensure that we are firing some markers
     */
    if( dtSettings.nMarkers == 0 )
    {
        fprintf( stdout, "Error: There are no markers to be activated.\n" );
        goto ERROR_EXIT;
    } /* if */

    /*
     * allocate memory for 3d data
     */
    p3dData = (Position3d*)malloc( dtSettings.nMarkers * sizeof( Position3d ) );

    /*
     * Configure Optotrak Collection
     * Once the system strobers have been enabled, and all settings are
     * loaded, the application can set up the Optotrak collection
     */
    fprintf( stdout, "...OptotrakSetupCollection\n" );
    fprintf( stdout, ".....%d, %.2f, %.0f, %d, %d, %d, %.2f, %.2f, %.0f, %.0f\n",
             dtSettings.nMarkers,
             dtSettings.fFrameFrequency,
             dtSettings.fMarkerFrequency,
             dtSettings.nThreshold,
             dtSettings.nMinimumGain,
             dtSettings.nStreamData,
             dtSettings.fDutyCycle,
             dtSettings.fVoltage,
             dtSettings.fCollectionTime,
             dtSettings.fPreTriggerTime );
    if( OptotrakSetupCollection( dtSettings.nMarkers,
                                 dtSettings.fFrameFrequency,
                                 dtSettings.fMarkerFrequency,
                                 dtSettings.nThreshold,
                                 dtSettings.nMinimumGain,
                                 dtSettings.nStreamData,
                                 dtSettings.fDutyCycle,
                                 dtSettings.fVoltage,
                                 dtSettings.fCollectionTime,
                                 dtSettings.fPreTriggerTime,
                                 OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG | OPTOTRAK_SWITCH_AND_CONFIG_FLAG ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Prepare for realtime data retrieval.
     * Activate markers. Turn on the markers prior to data retrieval.
     */
    fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers( ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */
    sleep( 1 );

    /*
     * Get and display five frames of 3D data.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "\n\n3D Data Display\n" );
    for( nCurFrame = 0; nCurFrame < 5; nCurFrame++ )
    {
        /*
         * Get a frame of data.
         */
        fprintf( stdout, "\n" );
        if( DataGetLatest3D( &uFrameNumber, &uElements, &uFlags, p3dData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the data.
         */
        fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
        fprintf( stdout, "Elements    : %8u\n", uElements );
        fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
        for( nCurMarker = 0; nCurMarker < dtSettings.nMarkers; nCurMarker++ )
        {
            DisplayMarker( nCurMarker + 1, p3dData[nCurMarker] );
        } /* for */

    } /* for */
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
    fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Stop the collection.
     */
    fprintf( stdout, "...OptotrakStopCollection\n" );
    if( OptotrakStopCollection( ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */

    fprintf( stdout, "Run again? [y|n] : " );
    gets( szInput );
    if( szInput[0] == 'y' )
    {
        goto EXECUTION_START;
    } /* if */

PROGRAM_COMPLETE:
    /*
     * CLEANUP
     */
    fprintf( stdout, "\n" );
    fprintf( stdout, "...TransputerShutdownSystem\n" );
    TransputerShutdownSystem( );

    /*
     * free all memory
     */
    if( pdtDeviceHandlesInfo )
    {
        for( i = 0; i < nDevices; i++ )
        {
            AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[i].grProperties), 0 );
        } /* for */
    } /* if */
    AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
    AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );
    free( p3dData );

    exit( 0 );


ERROR_EXIT:
    /*
     * Indicate that an error has occurred
     */
    fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString, MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

    fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers( );
    TransputerShutdownSystem( );

    /*
     * free all memory
     */
    if( pdtDeviceHandlesInfo )
    {
        for( i = 0; i < nDevices; i++ )
        {
            AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[i].grProperties), 0 );
        } /* for */
    } /* if */
    AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
    AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );
    free( p3dData );

    exit( 1 );

} /* main */
예제 #5
0
void main( int argc, unsigned char *argv[] )
{
	OptotrakSettings
		dtSettings;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];
    int
		i,
		nVLED,
		nMarkersToActivate,
		nCurDevice,
		nDevices;
	ApplicationDeviceInformation
		*pdtDevices;
    DeviceHandle
        *pdtDeviceHandles;
    DeviceHandleInfo
        *pdtDeviceHandlesInfo;


    /*
     * initialization
	 * intialize variables
     */
	pdtDevices = NULL;
	pdtDeviceHandles = NULL;
	pdtDeviceHandlesInfo = NULL;
	nDevices = 0;
	dtSettings.nMarkers = 0;
	dtSettings.fFrameFrequency = SAMPLE_FRAMEFREQ;
	dtSettings.fMarkerFrequency = SAMPLE_MARKERFREQ;
	dtSettings.nThreshold = 30;
	dtSettings.nMinimumGain = 160;
	dtSettings.nStreamData = SAMPLE_STREAMDATA;
	dtSettings.fDutyCycle = SAMPLE_DUTYCYCLE;
	dtSettings.fVoltage = SAMPLE_VOLTAGE;
	dtSettings.fCollectionTime = 1.0;
	dtSettings.fPreTriggerTime = 0.0;

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak Certus sample program #11\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Communication Initialization
     * Once the system processors have been loaded, the application
     * prepares for communication by initializing the system processors.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * Determine if this sample will run with the system attached.
	 * This sample is intended for Optotrak Certus systems.
	 */
	fprintf( stdout, "...DetermineSystem\n" );
	if( uDetermineSystem( ) != OPTOTRAK_CERTUS_FLAG )
	{
		goto PROGRAM_COMPLETE;
	} /* if */

    /*
     * Strober Initialization
     * Once communication has been initialized, the application must
     * determine the strober configuration.
     * The application retrieves device handles and all strober
     * properties from the system.
     */
	fprintf( stdout, "...DetermineStroberConfiguration\n" );
	if( DetermineStroberConfiguration( &pdtDeviceHandles, &pdtDeviceHandlesInfo, &nDevices ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * check if any devices have been detected by the system
	 */
	if( nDevices == 0 )
	{
		fprintf( stdout, ".........no devices detected.  Quitting program...\n" );
		goto PROGRAM_COMPLETE;
	} /* if */

    /*
     * Now that all the device handles have been completely set up,
     * the application can store all the device handle information in
     * an internal data structure.  This will facilitate lookups when
     * a property setting needs to be checked.
     */
    ApplicationStoreDeviceProperties( &pdtDevices, pdtDeviceHandlesInfo, nDevices );

	/*
	 * Change the number of markers to fire for all devices
	 */
	for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
	{
		nMarkersToActivate = pdtDevices[nCurDevice].b3020Capability?
							   CERTUS_SAMPLE_3020_STROBER_MARKERSTOFIRE : CERTUS_SAMPLE_STROBER_MARKERSTOFIRE;

		SetMarkersToActivateForDevice( &(pdtDevices[nCurDevice]), pdtDeviceHandlesInfo[nCurDevice].pdtHandle->nID, nMarkersToActivate );
	} /* if */
	fprintf( stdout, "\n" );

	/*
	 * Determine the collection settings based on the device properties
	 */
	ApplicationDetermineCollectionParameters( nDevices, pdtDevices, &dtSettings );

    /*
     * Set optional processing flags (this overides the settings in Optotrak.INI).
     */
	fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
    if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
                                    OPTO_CONVERT_ON_HOST |
                                    OPTO_RIGID_ON_HOST ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
	 * Load camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

    /*
     * Configure Optotrak Collection
     * Once the system strobers have been enabled, and all settings are
     * loaded, the application can set up the Optotrak collection
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
	fprintf( stdout, ".....%d, %.2f, %.0f, %d, %d, %d, %.2f, %.2f, %.0f, %.0f\n",
								 dtSettings.nMarkers,
			                     dtSettings.fFrameFrequency,
				                 dtSettings.fMarkerFrequency,
					             dtSettings.nThreshold,
						         dtSettings.nMinimumGain,
							     dtSettings.nStreamData,
								 dtSettings.fDutyCycle,
								 dtSettings.fVoltage,
								 dtSettings.fCollectionTime,
								 dtSettings.fPreTriggerTime );
    if( OptotrakSetupCollection( dtSettings.nMarkers,
			                     dtSettings.fFrameFrequency,
				                 dtSettings.fMarkerFrequency,
					             dtSettings.nThreshold,
						         dtSettings.nMinimumGain,
							     dtSettings.nStreamData,
								 dtSettings.fDutyCycle,
								 dtSettings.fVoltage,
								 dtSettings.fCollectionTime,
								 dtSettings.fPreTriggerTime,
								 OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG | OPTOTRAK_SWITCH_AND_CONFIG_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Prepare for realtime data retrieval.
     * Activate markers. Turn on the markers prior to data retrieval.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers( ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

	fprintf( stdout, "...waiting for 2 seconds\n" );
	sleep( 2 );

	/*
	 * Look through all devices to find visible LEDs
	 */
	fprintf( stdout, "\nProgram Results:\n" );
	for( i = 0; i < nDevices; i++ )
	{
		if( pdtDevices[i].nVLEDs == 0 )
		{
			fprintf( stdout, "...Device %d has no LEDs.  skipping...\n", i + 1 );
			continue;
		} /* if */

		/*
		 * Turn every visible LED in the device on, except for the first VLED
		 * NOTE: The first VLED is reserved by the strober so we do not try to set it.
		 */
		if( pdtDevices[i].nVLEDs == 1 )
		{
			fprintf( stdout, "...Device %d has only 1 LED.  skipping...\n", i + 1 );
			continue;
		} /* if */

		for( nVLED = 2; nVLED <= pdtDevices[i].nVLEDs; nVLED++ )
		{
			fprintf( stdout, "...OptotrakDeviceHandleSetVisibleLED (Device %d, LED %d)\n", i + 1, nVLED );
			if( OptotrakDeviceHandleSetVisibleLED( pdtDeviceHandlesInfo[i].pdtHandle->nID,
												   nVLED, VLEDST_ON ) != OPTO_NO_ERROR_CODE )
			{
				goto ERROR_EXIT;
			} /* if */
		} /* for */
	} /* for */

	fprintf( stdout, "...waiting for 5 seconds.  LEDs should be ON\n\n" );
	sleep( 5 );

	/*
	 * now make all the LEDs blink
	 */
	for( i = 0; i < nDevices; i++ )
	{
		if( pdtDevices[i].nVLEDs == 0 )
		{
			continue;
		} /* if */

		/*
		 * Make every visible LED in the device blink, except for the first VLED
		 * NOTE: The first VLED is reserved by the strober so we do not try to set it.
		 */
		if( pdtDevices[i].nVLEDs == 1 )
		{
			continue;
		} /* if */

		for( nVLED = 2; nVLED <= pdtDevices[i].nVLEDs; nVLED++ )
		{
			fprintf( stdout, "...OptotrakDeviceHandleSetVisibleLED (Device %d, LED %d)\n", i + 1, nVLED );
			if( OptotrakDeviceHandleSetVisibleLED( pdtDeviceHandlesInfo[i].pdtHandle->nID,
												   nVLED, VLEDST_BLINK ) != OPTO_NO_ERROR_CODE )
			{
				goto ERROR_EXIT;
			} /* if */
		} /* for */
	} /* for */

	fprintf( stdout, "...waiting for 5 seconds.  LEDs should be BLINKING\n\n" );
	sleep( 5 );

	/*
	 * finally, turn all LEDs off
	 */
	for( i = 0; i < nDevices; i++ )
	{
		if( pdtDevices[i].nVLEDs == 0 )
		{
			continue;
		} /* if */

		/*
		 * Turn every visible LED in the device off, except for the first VLED
		 * NOTE: The first VLED is reserved by the strober so we do not try to set it.
		 */
		if( pdtDevices[i].nVLEDs == 1 )
		{
			continue;
		} /* if */

		for( nVLED = 2; nVLED <= pdtDevices[i].nVLEDs; nVLED++ )
		{
			fprintf( stdout, "...OptotrakDeviceHandleSetVisibleLED (Device %d, LED %d)\n", i + 1, nVLED );
			if( OptotrakDeviceHandleSetVisibleLED( pdtDeviceHandlesInfo[i].pdtHandle->nID,
												   nVLED, VLEDST_OFF ) != OPTO_NO_ERROR_CODE )
			{
				goto ERROR_EXIT;
			} /* if */
		} /* for */
	} /* for */

	fprintf( stdout, "...waiting for 5 seconds.  LEDs should be OFF\n\n" );
	sleep( 5 );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

PROGRAM_COMPLETE:
    /*
     * CLEANUP
     */
	fprintf( stdout, "\n" );
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers( );
    TransputerShutdownSystem( );
	exit( 0 );


ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString, MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
	OptotrakDeActivateMarkers( );
	TransputerShutdownSystem( );

    exit( 1 );

} /* main */
예제 #6
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    unsigned int
        uFlags,
        uElements,
        uFrameCnt,
        uChannelCnt,
        uFrameNumber;
    static int
        puRawData[ NUM_CHANNELS + 1];
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak sample program #12\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Wait one second to let the system finish loading.
     */
    sleep( 1 );

    /*
     * Initialize the processors system.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the ODAU.
     */
	fprintf( stdout, "...OdauSetupCollection\n" );
    if( OdauSetupCollection(
            ODAU1,                  /* Id the ODAU the parameters are for. */
            NUM_CHANNELS,           /* Number of analog channels to collect. */
            ODAU_GAIN,              /* Gain to use for the analog channels.*/
            ODAU_DIGITAL_INPB_INPA, /* Mode for the Digital I/O port.*/
            (float)100.0,           /* Frequency to collect data frames at. */
            (float)90000.0,         /* Frequency to scan channels at. */
            0,                      /* Stream mode for the data buffers. */
            (float)1.0,             /* Number of seconds of data to collect. */
            0.0,                    /* Number of seconds to pre-trigger data by. */
            0 ) )                   /* Flags. */
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the Optotrak.
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
    if( OptotrakSetupCollection(
            NUM_MARKERS,    /* Number of markers in the collection. */
            (float)100.0,   /* Frequency to collect data frames at. */
            (float)2500.0,  /* Marker frequency for marker maximum on-time. */
            30,             /* Dynamic or Static Threshold value to use. */
            160,            /* Minimum gain code amplification to use. */
            0,              /* Stream mode for the data buffers. */
            (float)0.4,    /* Marker Duty Cycle to use. */
            (float)7.5,     /* Voltage to use when turning on markers. */
            (float)1.0,     /* Number of seconds of data to collect. */
            (float)0.0,     /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Get and display ten frames of ODAU data.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "ODAU Data Display\n" );
    for( uFrameCnt = 0; uFrameCnt < 10; ++uFrameCnt )
    {

        /*
         * Get a frame of ODAU raw data.
         */
        fprintf( stdout, "\n" );
        if( DataGetLatestOdauRaw( ODAU1, &uFrameNumber, &uElements, &uFlags,
            puRawData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the data.
         */
        fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
        fprintf( stdout, "Elements    : %8u\n", uElements );
        fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
        for( uChannelCnt = 0; uChannelCnt < NUM_CHANNELS; ++uChannelCnt )
        {
            fprintf( stdout, "Channel %u  Raw 0x%08x  Voltage %f\n",
                     uChannelCnt + 1,
                     puRawData[ uChannelCnt],
                     (float)( (int)( puRawData[ uChannelCnt])) *
                                     0.000305175/(float)ODAU_GAIN );
            /*
             * if you have an ODAUI unit, instead of an ODAUII unit,
             * replace the above fprintf statement with:
             * fprintf( stdout, "Channel %u  Raw 0x%08x  Voltage %f\n",
             *          uChannelCnt + 1,
             *          puRawData[ uChannelCnt ],
             *          (float)( (int)( puRawData[ uChannelCnt] & 0x0FFF ) - 2048) *
             *                          (10.0/2048.0)/(float)ODAU_GAIN );
             */
        } /* for */
        fprintf( stdout, "Digital 0x%04x\n", puRawData[ NUM_CHANNELS] );
    } /* for */
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Shutdown the processors message passing system.
     */
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    fprintf( stdout, "\nProgram execution complete.\n" );
    exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();

    exit( 1 );

} /* main */
예제 #7
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    unsigned int
        uFlags,
        uElements,
        uFrameCnt,
        uRigidCnt,
        uMarkerCnt,
        uFrameNumber;
    RigidBodyDataType
        RigidBodyData;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

    /*
     * Load the system of transputers.
     */
    if( TransputerLoadSystem( "system" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the system finish loading.
     */
    sleep( 1 );

    /*
     * Initialize the transputer system.
     */
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the OPTOTRAK.
     */
    if( OptotrakSetupCollection(
            NUM_MARKERS,        /* Number of markers in the collection. */
            FRAME_RATE,         /* Frequency to collect data frames at. */
            (float)2500.0,      /* Marker frequency for marker maximum on-time. */
            30,                 /* Dynamic or Static Threshold value to use. */
            160,                /* Minimum gain code amplification to use. */
            1,                  /* Stream mode for the data buffers. */
            (float)0.4,         /* Marker Duty Cycle to use. */
            (float)7.5,         /* Voltage to use when turning on markers. */
            COLLECTION_TIME,    /* Number of seconds of data to collect. */
            (float)0.0,         /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_BUFFER_RAW_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Add rigid body 1 for tracking to the OPTOTRAK system from a .RIG file.
     */
    if( RigidBodyAddFromFile(
            RIGID_BODY_1,   /* ID associated with this rigid body. */
            1,              /* First marker in the rigid body.*/
            "plate",        /* RIG file containing rigid body coordinates.*/
            0 ) )           /* flags */
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Change the default settings for this rigid body 1.
     */
    if( RigidBodyChangeSettings(
            RIGID_BODY_1,   /* ID associated with this rigid body. */
            4,              /* Minimum number of markers which must be seen
                               before performing rigid body calculations.*/
            60,             /* Cut off angle for marker inclusion in calcs.*/
            (float)0.25,    /* Maximum 3-D marker error for this rigid body. */
            (float)1.0,     /* Maximum raw sensor error for this rigid body. */
            (float)1.0,     /* Maximum 3-D RMS marker error for this rigid body. */
            (float)1.0,     /* Maximum raw sensor RMS error for this rigid body. */
            OPTOTRAK_QUATERN_RIGID_FLAG | OPTOTRAK_RETURN_QUATERN_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Get and display ten frames of rigid body data.
     */
    fprintf( stdout, "Rigid Body Data Display\n" );
    for( uFrameCnt = 0; uFrameCnt < 10; ++uFrameCnt )
    {

        /*
         * Get a frame of data.
         */
        if( DataGetLatestTransforms( &uFrameNumber, &uElements, &uFlags,
                                     &RigidBodyData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the rigid body transformation data.
         */
        fprintf( stdout, "\n" );
        fprintf( stdout, "Rigid Body Transformation Data\n\n" );
        fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
        fprintf( stdout, "Transforms  : %8u\n", uElements );
        fprintf( stdout, "Flags       :   0x%04x\n", uFlags );
        for( uRigidCnt = 0; uRigidCnt < uElements; ++uRigidCnt )
        {
            fprintf( stdout, "Rigid Body %u\n",
                     RigidBodyData.pRigidData[ uRigidCnt].RigidId );
            fprintf( stdout, "XT = %8.2f YT = %8.2f ZT = %8.2f\n",
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.translation.x,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.translation.y,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.translation.z );
            fprintf( stdout, "Q0 = %8.2f QX = %8.2f QY = %8.2f QZ = %8.2f\n",
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.rotation.q0,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.rotation.qx,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.rotation.qy,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         quaternion.rotation.qz );
        } /* for */

        /*
         * Print out the 3D data.
         */
        fprintf( stdout, "\nAssociated 3D Marker Data\n\n" );
        for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; ++uMarkerCnt )
        {
            fprintf( stdout, "Marker %u X %f Y %f Z %f\n",
                     uMarkerCnt + 1,
                     RigidBodyData.p3dData[ uMarkerCnt].x,
                     RigidBodyData.p3dData[ uMarkerCnt].y,
                     RigidBodyData.p3dData[ uMarkerCnt].z );
        } /* for */
    } /* for */

    /*
     * De-activate the markers.
     */
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Shutdown the transputer message passing system.
     */
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    fprintf( stdout, "\nProgram execution complete.\n" );
    exit( 0 );

ERROR_EXIT:
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();
    exit( 1 );

} /* main */
예제 #8
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    float
        fRmsError;
    boolean
        bValid = TRUE;
    unsigned int
        uFlags,
        uElements,
		uFramesToAvg,
        uFrameCnt,
        uMarkerCnt,
        uFrameNumber;
    static Position3d
        grdt3dData[NUM_MARKERS],
        grdtAveraged3dData[NUM_MARKERS];
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

    /*
     * Initialize the averaged marker position array to zeros.
     */
    for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; uMarkerCnt++ )
    {
        grdtAveraged3dData[uMarkerCnt].x =
        grdtAveraged3dData[uMarkerCnt].y =
        grdtAveraged3dData[uMarkerCnt].z = (float) 0.0;
    } /* for */

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak sample program #18\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Wait one second to let the system finish loading.
     */
    sleep( 1 );

    /*
     * Initialize the processors system.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the Optotrak.
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
    if( OptotrakSetupCollection(
            NUM_MARKERS,    /* Number of markers in the collection. */
            (float)100.0,   /* Frequency to collect data frames at. */
            (float)2500.0,  /* Marker frequency for marker maximum on-time. */
            30,             /* Dynamic or Static Threshold value to use. */
            160,            /* Minimum gain code amplification to use. */
            0,              /* Stream mode for the data buffers. */
            (float)0.35,    /* Marker Duty Cycle to use. */
            (float)7.0,     /* Voltage to use when turning on markers. */
            (float)1.0,     /* Number of seconds of data to collect. */
            (float)0.0,     /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Average 50 frames of 3D data to a single frame.
     */
    uFrameCnt = 0;
	uFramesToAvg = 0;
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "\n\nAveraging 3D data...\n" );
    do
    {
        /*
         * Get a frame of data.
         */
        if( DataGetLatest3D( &uFrameNumber, &uElements, &uFlags, grdt3dData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Determine if the frame is valid; that is if each marker has a 3D
         * coordinate.
         */
        bValid = TRUE;
        for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; uMarkerCnt++ )
        {
            if( grdt3dData[uMarkerCnt].x < MAX_NEGATIVE )
            {
                bValid = FALSE;
            } /* if */
        } /* for */

        /*
         * Add in each markers contribution if this frame was valid.
         */
        if( bValid )
        {
            for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; uMarkerCnt++ )
            {
                grdtAveraged3dData[uMarkerCnt].x += grdt3dData[uMarkerCnt].x;
                grdtAveraged3dData[uMarkerCnt].y += grdt3dData[uMarkerCnt].y;
                grdtAveraged3dData[uMarkerCnt].z += grdt3dData[uMarkerCnt].z;
				uFramesToAvg++;
            } /* for */
            ++uFrameCnt;
        } /* if */
    } /* do */
    while( uFrameCnt < 50 );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Divide sums by 50 to get the averaged marker positions.
     */
    for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; uMarkerCnt++ )
    {
        grdtAveraged3dData[uMarkerCnt].x /= uFramesToAvg;
        grdtAveraged3dData[uMarkerCnt].y /= uFramesToAvg;
        grdtAveraged3dData[uMarkerCnt].z /= uFramesToAvg;
    } /* for */

    /*
     * Transform the measurement frame of reference for the Optotrak system
     * to the frame of reference defined by the grdtAlignedPositions array.
     */
    if( OptotrakChangeCameraFOR(
            "standard",             /* Camera Parameter File to load transform. */
            NUM_MARKERS,            /* Number of markers in 3D arrays. */
            grdtAlignedPositions,   /* Marker positions in current FOR. */
            grdtAveraged3dData,     /* Marker positions in desired FOR. */
            "new",                  /* Name of new camera parameter file. */
            grdt3dErrors,           /* 3D errors of alignment transformation. */
            &fRmsError ) )          /* RMS distance error of alignment transformation.*/
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Print out the results of the alignment.
     */
    fprintf( stdout, "RMS Fit Error : %6.3f\n", fRmsError );
    fprintf( stdout, "Coordinate Fit Error Table\n" );
    fprintf( stdout, "Mkr %6s %6s %6s\n","X", "Y", "Z" );
    for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; ++uMarkerCnt )
    {
		DisplayMarker( uMarkerCnt + 1, grdt3dErrors[uMarkerCnt] );
    } /* for */

    /*
     * Load the NEW camera parameter file to the Optotrak system.
     */
    fprintf( stdout, "\nLoading NEW camera parameters...\n\n" );
    if( OptotrakLoadCameraParameters( "NEW" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Get and display ten frames of 3D data.  The position of which will be
     * in the new frame of reference.
     */
    fprintf( stdout, "\n\n3D Data Display\n" );
    for( uFrameCnt = 0; uFrameCnt < 10; ++uFrameCnt )
    {
        /*
         * Get a frame of data.
         */
        fprintf( stdout, "\n" );
        if( DataGetLatest3D( &uFrameNumber, &uElements, &uFlags, grdt3dData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the data.
         */
        fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
        fprintf( stdout, "Elements    : %8u\n", uElements );
        fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
        for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; uMarkerCnt++ )
        {
			DisplayMarker( uMarkerCnt + 1, grdt3dData[uMarkerCnt] );
        } /* for */
    } /* for */
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Shutdown the processors message passing system.
     */
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    fprintf( stdout, "\nProgram execution complete.\n" );
    exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();

    exit( 1 );

} /* main */
예제 #9
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    unsigned int
        uFlags,
        uElements,
        uFrameCnt,
        uRigidCnt,
        uMarkerCnt,
        uFrameNumber;
    RigidBodyDataType
        RigidBodyData;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak sample program #11\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Wait one second to let the system finish loading.
     */
    sleep( 1 );

    /*
     * Initialize the processors system.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set optional processing flags (this overides the settings in Optotrak.INI).
     */
	fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
    if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
                                    OPTO_CONVERT_ON_HOST |
                                    OPTO_RIGID_ON_HOST ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the Optotrak.
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
    if( OptotrakSetupCollection(
            NUM_MARKERS,        /* Number of markers in the collection. */
            FRAME_RATE,         /* Frequency to collect data frames at. */
            (float)2500.0,      /* Marker frequency for marker maximum on-time. */
            30,                 /* Dynamic or Static Threshold value to use. */
            160,                /* Minimum gain code amplification to use. */
            1,                  /* Stream mode for the data buffers. */
            (float)0.4,         /* Marker Duty Cycle to use. */
            (float)7.5,         /* Voltage to use when turning on markers. */
            COLLECTION_TIME,    /* Number of seconds of data to collect. */
            (float)0.0,         /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Add rigid body 2 for tracking to the Optotrak system from an array of
     * 3D points.
     */
	fprintf( stdout, "...RigidBodyAdd\n" );
    if( RigidBodyAdd(
            RIGID_BODY_2,           /* ID associated with this rigid body. */
            7,                      /* First marker in the rigid body. */
            6,                      /* Number of markers in the rigid body. */
            (float *)RigidBody2,    /* 3D coords for each marker in the body. */
            NULL,                   /* no normals for this rigid body. */
            OPTOTRAK_QUATERN_RIGID_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Add rigid body 1 for tracking to the Optotrak system from a .RIG file.
     */
	fprintf( stdout, "...RigidBodyAddFromFile\n" );
    if( RigidBodyAddFromFile(
            RIGID_BODY_1,   /* ID associated with this rigid body.*/
            1,              /* First marker in the rigid body.*/
            "plate",        /* RIG file containing rigid body coordinates.*/
            OPTOTRAK_QUATERN_RIGID_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Change the default frame of reference to be defined by rigid body one.
     */
	fprintf( stdout, "...RigidBodyChangeFOR\n" );
    if( RigidBodyChangeFOR( RIGID_BODY_1, OPTOTRAK_CONSTANT_RIGID_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Get and display ten frames of rigid body data.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "Rigid Body Data Display\n" );
    for( uFrameCnt = 0; uFrameCnt < 10; ++uFrameCnt )
    {
        /*
         * Get a frame of data.
         */
        if( DataGetLatestTransforms( &uFrameNumber, &uElements, &uFlags,
                                     &RigidBodyData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the rigid body transformation data.
         */
        fprintf( stdout, "\n" );
        fprintf( stdout, "Rigid Body Transformation Data\n\n" );
        fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
        fprintf( stdout, "Transforms  : %8u\n", uElements );
        fprintf( stdout, "Flags       :   0x%04x\n", uFlags );
        for( uRigidCnt = 0; uRigidCnt < uElements; ++uRigidCnt )
        {
            fprintf( stdout, "Rigid Body %u\n",
                     RigidBodyData.pRigidData[ uRigidCnt].RigidId );
            fprintf( stdout, "XT = %8.2f YT = %8.2f ZT = %8.2f\n",
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         euler.translation.x,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         euler.translation.y,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         euler.translation.z );
            fprintf( stdout, "Y  = %8.2f P  = %8.2f R  = %8.2f\n",
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         euler.rotation.yaw,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         euler.rotation.pitch,
                     RigidBodyData.pRigidData[ uRigidCnt].transformation.
                         euler.rotation.roll );
        } /* for */

        /*
         * Print out the 3D data.
         */
        fprintf( stdout, "\nAssociated 3D Marker Data\n\n" );
        for( uMarkerCnt = 0; uMarkerCnt < NUM_MARKERS; ++uMarkerCnt )
        {
			DisplayMarker( uMarkerCnt + 1, RigidBodyData.p3dData[uMarkerCnt] );
        } /* for */
    } /* for */
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Shutdown the processors message passing system.
     */
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    fprintf( stdout, "\nProgram execution complete.\n" );

    exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();

    exit( 1 );

} /* main */
예제 #10
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    unsigned int
        uSpoolStatus = 0;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak sample program #13\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Wait one second to let the system finish loading.
     */
    sleep( 1 );

    /*
     * Initialize the processors system.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the ODAU.
     */
	fprintf( stdout, "...OdauSetupCollection\n" );
    if( OdauSetupCollection(
            ODAU1,                  /* Id the ODAU the parameters are for. */
            NUM_CHANNELS,           /* Number of analog channels to collect. */
            ODAU_GAIN,              /* Gain to use for the analog channels.*/
            ODAU_DIGITAL_INPB_INPA, /* Mode for the Digital I/O port.*/
            (float)100.0,           /* Frequency to collect data frames at. */
            (float)90000.0,         /* Frequency to scan channels at. */
            1,                      /* Stream mode for the data buffers. */
            (float)2.0,             /* Number of seconds of data to collect. */
            (float)0.0,             /* Number of seconds to pre-trigger data by. */
            0 ) )                   /* Flags. */
    {
    goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the Optotrak.
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
    if( OptotrakSetupCollection(
            NUM_MARKERS,    /* Number of markers in the collection. */
            (float)50.0,    /* Frequency to collect data frames at. */
            (float)2500.0,  /* Marker frequency for marker maximum on-time. */
            30,             /* Dynamic or Static Threshold value to use. */
            160,            /* Minimum gain code amplification to use. */
            1,              /* Stream mode for the data buffers. */
            (float)0.40,    /* Marker Duty Cycle to use. */
            (float)8.0,     /* Voltage to use when turning on markers. */
            (float)2.0,     /* Number of seconds of data to collect. */
            (float)0.0,     /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_NO_FIRE_MARKERS_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Initialize a file for spooling of the Optotrak 3D data.
     */
	fprintf( stdout, "...DataBufferInitializeFile\n" );
    if( DataBufferInitializeFile( OPTOTRAK, "C#001.S13" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Initialize a file for spooling of the ODAU raw data.
     */
	fprintf( stdout, "...DataBufferInitializeFile\n" );
    if( DataBufferInitializeFile( ODAU1, "O1#001.S13" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Spool data to the previously initialized files.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "Collecting data files...\n" );
    if( DataBufferSpoolData( &uSpoolStatus ) )
    {
        goto ERROR_EXIT;
    } /* if */
    fprintf( stdout, "Spool Status: 0x%04x\n", uSpoolStatus );
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Shutdown the processors message passing system.
     */
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    fprintf( stdout, "\nProgram execution complete.\n" );
    exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();

    exit( 1 );

} /* main */
예제 #11
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    int
        nFileItems,
        nFileSubItems;
    void
        *pFileHeader;
    float
        fFileFrequency,
        *pfRawDataCur = NULL;
    long int
        lnFrameCnt,
        lnFileFrames;
    unsigned int
        uElements,
        uElementCnt,
        uSpoolStatus = 0;
    static char
        szFileComments[ 81];
    float
        *pfRawData = NULL;
    Position3d
        *pdtPositionData = NULL,
        *pdtPositionCur  = NULL;
    struct OptotrakRigidStruct
        dtRigidBodyData;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak sample program #19\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Wait one second to let the system finish loading.
     */
    sleep( 1 );

    /*
     * Initialize the processors system.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set optional processing flags (this overides the settings in Optotrak.INI).
     */
	fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
    if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
                                    OPTO_RIGID_ON_HOST ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the Optotrak.
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
    if( OptotrakSetupCollection(
            NUM_MARKERS,        /* Number of markers in the collection. */
            FRAME_RATE,         /* Frequency to collect data frames at. */
            (float)2500.0,      /* Marker frequency for marker maximum on-time. */
            30,                 /* Dynamic or Static Threshold value to use. */
            160,                /* Minimum gain code amplification to use. */
            0,                  /* Stream mode for the data buffers. */
            (float)0.4,         /* Marker Duty Cycle to use. */
            (float)7.5,         /* Voltage to use when turning on markers. */
            COLLECTION_TIME,    /* Number of seconds of data to collect. */
            (float)0.0,         /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG | OPTOTRAK_GET_NEXT_FRAME_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Add the rigid body to the Optotrak system from an array of 3D points.
     */
	fprintf( stdout, "...RigidBodyAdd\n" );
    if( RigidBodyAdd(
            RIGID_BODY_ID,          /* ID associated with this rigid body. */
            1,                      /* First marker in the rigid body. */
            NUM_MARKERS,            /* Number of markers in the rigid body. */
            (float *)dtRigidBody,   /* 3D coords for each marker in the body. */
            NULL,                   /* no normals for this rigid body. */
            OPTOTRAK_QUATERN_RIGID_FLAG | OPTOTRAK_RETURN_EULER_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */


    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Initialize a file for spooling of the Optotrak raw data.
     */
	fprintf( stdout, "...DataBufferInitializeFile\n" );
    if( DataBufferInitializeFile( OPTOTRAK, "R#001.S19" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Spool data to the previously initialized file.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "Collecting data file...\n" );
    if( DataBufferSpoolData( &uSpoolStatus ) )
    {
        goto ERROR_EXIT;
    } /* if */
    fprintf( stdout, "Spool Status: 0x%04x\n", uSpoolStatus );
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Open the raw data file we just collected as our input data file.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "Processing raw data file...\n" );
    if( FileOpen( "R#001.S19",
                  INPUT_FILE,
                  OPEN_READ,
                  &nFileItems,
                  &nFileSubItems,
                  &lnFileFrames,
                  &fFileFrequency,
                  szFileComments,
                  &pFileHeader ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Allocate the memory required for the conversions and transformations.
     */
    pfRawData = (float *)malloc( lnFileFrames * nFileItems * nFileSubItems *
                                 sizeof(float) );
    if( pfRawData == NULL )
    {
        fprintf( stderr, "Insufficient memory for raw data." );
        goto ERROR_EXIT;
    } /* if */
    pdtPositionData = (Position3d *)malloc( lnFileFrames * nFileItems *
                                            nFileSubItems * sizeof(Position3d) );
    if( pdtPositionData == NULL )
    {
        fprintf( stderr, "Insufficient memory for 3D position data." );
        goto ERROR_EXIT;
    } /* if */

    /*
     * Read the raw data from the file.
     */
    if( FileRead( INPUT_FILE, 0L, (int)lnFileFrames, pfRawData ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Close the input file.
     */
    FileClose( INPUT_FILE );
    fprintf( stdout, "File processing complete.\n" );

    /*
     * Convert the raw data to 3D position data and display it.
     */
    fprintf( stdout, "\n3D position data:" );
    pfRawDataCur   = pfRawData;
    pdtPositionCur = pdtPositionData;
    for( lnFrameCnt = 0; lnFrameCnt < lnFileFrames; lnFrameCnt++ )
    {
        if( OptotrakConvertRawTo3D( &uElements,
                                    pfRawDataCur,
                                    pdtPositionCur ) )
        {
            goto ERROR_EXIT;
        } /* if */

        for( uElementCnt = 0; uElementCnt < uElements; uElementCnt++ )
        {
            fprintf( stdout,
                    "\n%5ld %3u %12.5f %12.5f %12.5f",
                    lnFrameCnt,
                    uElementCnt,
                    pdtPositionCur[uElementCnt].x,
                    pdtPositionCur[uElementCnt].y,
                    pdtPositionCur[uElementCnt].z );
        } /* for */
        pfRawDataCur += uElements * NUM_SENSORS;
        pdtPositionCur += uElements;
    } /* for */

    /*
     * Transform the 3D position data to 6D and display it.
     */
    fprintf( stdout, "\n\nRigid body data:\n" );
    pdtPositionCur = pdtPositionData;
    for( lnFrameCnt = 0; lnFrameCnt < lnFileFrames; lnFrameCnt++ )
    {
        if( OptotrakConvertTransforms( &uElements,
                                       &dtRigidBodyData,
                                       pdtPositionCur ) )
        {
            goto ERROR_EXIT;
        } /* if */

        fprintf( stdout, "Frame %04u\n", lnFrameCnt );
        fprintf( stdout,
                 "XT = %12.6f YT = %12.6f ZT = %12.6f\n",
                 dtRigidBodyData.transformation.euler.translation.x,
                 dtRigidBodyData.transformation.euler.translation.y,
                 dtRigidBodyData.transformation.euler.translation.z );
        fprintf( stdout,
                 "Y  = %12.6f P  = %12.6f R  = %12.6f\n",
                 dtRigidBodyData.transformation.euler.rotation.yaw,
                 dtRigidBodyData.transformation.euler.rotation.pitch,
                 dtRigidBodyData.transformation.euler.rotation.roll );
        pdtPositionCur += uElementCnt;
    } /* for */
    fprintf( stdout, "\n" );

    /*
     * Shutdown the processors message passing system.
     */
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    free( pfRawData );
    free( pdtPositionData );
    fprintf( stdout, "\nProgram execution complete.\n" );
    exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();

    if( pfRawData != NULL )
	{
        free( pfRawData );
	} /* if */

    if( pdtPositionData != NULL )
	{
        free( pdtPositionData );
	} /* if */

    exit( 1 );
} /* main */
예제 #12
0
/*****************************************************************
Name:               main

Input Values:
    int
        argc        :Number of command line parameters.
    unsigned char
        *argv[]     :Pointer array to each parameter.

Output Values:
    None.

Return Value:
    None.

Description:

    Main program routine performs all steps listed in the above
    program description.

*****************************************************************/
void main( int argc, unsigned char *argv[] )
{
    int
        nSensorCode;
    unsigned int
        uFlags,
        uElements,
        uFrameCnt,
        uMarkerCnt,
        uSensorCnt,
        uFrameNumber,
        uSpoolStatus = 0;
    static FullRawDataType
        pFullRawData[ NUM_MARKERS];
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak sample program #3\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Initialize the processors system.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Load the standard camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Set up a collection for the Optotrak.
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
    if( OptotrakSetupCollection(
            NUM_MARKERS,    /* Number of markers in the collection. */
            (float)50.0,    /* Frequency to collect data frames at. */
            (float)2500.0,  /* Marker frequency for marker maximum on-time. */
            30,             /* Dynamic or Static Threshold value to use. */
            160,            /* Minimum gain code amplification to use. */
            1,              /* Stream mode for the data buffers. */
            (float)0.4,     /* Marker Duty Cycle to use. */
            (float)7.5,     /* Voltage to use when turning on markers. */
            (float)2.0,     /* Number of seconds of data to collect. */
            (float)0.0,     /* Number of seconds to pre-trigger data by. */
            OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_GET_NEXT_FRAME_FLAG ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Activate the markers.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    /*
     * Get and display ten frames of full raw data.
     */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    for( uFrameCnt = 0; uFrameCnt < 10; ++uFrameCnt )
    {
        /*
         * Get a frame of data.
         */
        if( DataGetLatestRaw( &uFrameNumber, &uElements, &uFlags,
                              pFullRawData ) )
        {
            goto ERROR_EXIT;
        } /* if */

        /*
         * Print out the data.
         */
        fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
        fprintf( stdout, "Elements    : %8u\n", uElements );
        fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
        for( uMarkerCnt = 0; uMarkerCnt < 6; ++uMarkerCnt )
        {

            /*
             * Print out the current marker number.
             */
            fprintf( stdout, "Marker %u\t\tCentroid Peak  DRC Code\n",
                uMarkerCnt + 1 );

            /*
             * Print out the information for each sensor.
             */
            for( uSensorCnt = 0; uSensorCnt < NUM_SENSORS; ++uSensorCnt )
            {

                /*
                 * Print out the current sensor number.
                 */
                fprintf( stdout, "\tSensor %u\t", uSensorCnt + 1 );

                /*
                 * Print out the centroid.  If is is bad print out the
                 * string 'missing'.
                 */
                if( pFullRawData[ uMarkerCnt].fCentroid[ uSensorCnt] <
                    MAX_NEGATIVE )
                {
                    fprintf( stdout, " missing " );
                } /* if */
                else
                {
                    fprintf( stdout, "%8.2f ",
                        pFullRawData[ uMarkerCnt].fCentroid[ uSensorCnt] );
                } /* else */

                /*
                 * Print out the rest of this sensor's information.
                 */
                nSensorCode =
                    pFullRawData[ uMarkerCnt].SensorData[ uSensorCnt].ucCode;
                fprintf( stdout, "%4d %4d %s\n",
                    pFullRawData[ uMarkerCnt].SensorData[ uSensorCnt].ucPeak,
                    pFullRawData[ uMarkerCnt].SensorData[ uSensorCnt].ucDRC,
                    pSensorStatusString[ nSensorCode] );
            } /* for */
        } /* for */
    } /* for */
    fprintf( stdout, "\n" );

    /*
     * Initialize a data file for spooling of the Optotrak data.
     */
    fprintf( stdout, "...DataBufferInitializeFile\n" );
    if( DataBufferInitializeFile( OPTOTRAK, "C#001.S03" ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Spool data to the previously initialized file.
     */
    fprintf( stdout, "...DataBufferSpoolData\n" );
    if( DataBufferSpoolData( &uSpoolStatus ) )
    {
        goto ERROR_EXIT;
    } /* if */
    fprintf( stdout, "\n\nSample Program Results:\n\n" );
    fprintf( stdout, "Spool Status: 0x%04x\n", uSpoolStatus );
    fprintf( stdout, "\n" );

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Shutdown the processors message passing system.
     */
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    if( TransputerShutdownSystem() )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
     * Exit the program.
     */
    fprintf( stdout, "\nProgram execution complete.\n" );
    exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString,
                                MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers();
    TransputerShutdownSystem();

    exit( 1 );

} /* main */
예제 #13
0
void main( int argc, unsigned char *argv[] )
{
	OptotrakSettings
		dtSettings;
    int
		i,
		nIterations,
		nCurDevice,
		nCurMarker,
		nDevices,
		nMarkersToActivate;
	ApplicationDeviceInformation
		*pdtDevices;
    DeviceHandle
        *pdtDeviceHandles;
    DeviceHandleInfo
        *pdtDeviceHandlesInfo;
	unsigned int
		uFlags,
		uElements,
		uFrameNumber;
    Position3d
        *p3dData;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];


    /*
     * initialization
	 * intialize variables
     */
	pdtDevices = NULL;
	pdtDeviceHandles = NULL;
	pdtDeviceHandlesInfo = NULL;
	p3dData = NULL;
	dtSettings.nMarkers = 0;
	dtSettings.fFrameFrequency = SAMPLE_FRAMEFREQ;
	dtSettings.fMarkerFrequency = SAMPLE_MARKERFREQ;
	dtSettings.nThreshold = 30;
	dtSettings.nMinimumGain = 160;
	dtSettings.nStreamData = SAMPLE_STREAMDATA;
	dtSettings.fDutyCycle = SAMPLE_DUTYCYCLE;
	dtSettings.fVoltage = SAMPLE_VOLTAGE;
	dtSettings.fCollectionTime = 1.0;
	dtSettings.fPreTriggerTime = 0.0;
	nDevices = 0;
	nMarkersToActivate = 0;

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak Certus sample program #2\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Communication Initialization
     * Once the system processors have been loaded, the application
     * prepares for communication by initializing the system processors.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * Determine if this sample will run with the system attached.
	 * This sample is intended for Optotrak Certus systems.
	 */
	fprintf( stdout, "...DetermineSystem\n" );
	if( uDetermineSystem( ) != OPTOTRAK_CERTUS_FLAG )
	{
		goto ERROR_EXIT;
	} /* if */

	nIterations = 2;
	for( i = 0; i < nIterations; i++ )
	{
		/*
		 * Strober Initialization
		 * Once communication has been initialized, the application must
		 * determine the strober configuration.
		 * The application retrieves device handles and all strober
		 * properties from the system.
		 */
		fprintf( stdout, "...DetermineStroberConfiguration\n" );
		if( DetermineStroberConfiguration( &pdtDeviceHandles, &pdtDeviceHandlesInfo, &nDevices ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		/*
		 * check if any devices have been detected by the system
		 */
		if( nDevices == 0 )
		{
			fprintf( stdout, ".........no devices detected.  Quitting program...\n" );
			goto PROGRAM_COMPLETE;
		} /* if */

		/*
		 * Now that all the device handles have been completely set up,
		 * the application can store all the device handle information in
		 * an internal data structure.  This will facilitate lookups when
		 * a property setting needs to be checked.
		 */
		ApplicationStoreDeviceProperties( &pdtDevices, pdtDeviceHandlesInfo, nDevices );

		/*
		 * Change the number of markers to fire for all devices
		 */
		for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
		{
			nMarkersToActivate = pdtDevices[nCurDevice].b3020Capability?
								   CERTUS_SAMPLE_3020_STROBER_MARKERSTOFIRE : CERTUS_SAMPLE_STROBER_MARKERSTOFIRE;

			SetMarkersToActivateForDevice( &(pdtDevices[nCurDevice]), pdtDeviceHandlesInfo[nCurDevice].pdtHandle->nID, nMarkersToActivate );
		} /* for */
		fprintf( stdout, "\n" );

		/*
		 * If there are strobers with tool capabilities, but no tools,
		 * then set up the strobers to fire some markers
		 */
		for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
		{
			if( pdtDevices[nCurDevice].nToolPorts > 0 )
			{
				/*
				 * set the markers to activate for this device
				 * this will be 0 if there are tools plugged into the
				 * device, or CERTUS_SAMPLE_STROBER_MARKERSTOFIRE if there
				 * are no tools plugged in
				 */
				nMarkersToActivate = CERTUS_SAMPLE_STROBER_MARKERSTOFIRE;
				SetMarkersToActivateForToolDevice( &(pdtDevices[nCurDevice]), pdtDeviceHandlesInfo[nCurDevice].pdtHandle->nID, nMarkersToActivate, pdtDevices, nDevices );
			} /* if */
		} /* for */

		/*
		 * Determine the collection settings based on the device properties
		 */
		ApplicationDetermineCollectionParameters( nDevices, pdtDevices, &dtSettings );


		/*
		 * Set optional processing flags (this overides the settings in Optotrak.INI).
		 */
		fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
		if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
										OPTO_CONVERT_ON_HOST |
										OPTO_RIGID_ON_HOST ) )
		{
			goto ERROR_EXIT;
		} /* if */

		/*
		 * Load camera parameters.
		 */
		fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
		if( OptotrakLoadCameraParameters( "standard" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		/*
		 * Ensure that we are firing some markers
		 */
		if( dtSettings.nMarkers == 0 )
		{
			fprintf( stdout, "Error: There are no markers to be activated.\n" );
			goto ERROR_EXIT;
		} /* if */

		/*
		 * allocate memory for 3d data
		 */
		p3dData = (Position3d*)malloc( dtSettings.nMarkers * sizeof( Position3d ) );

		/*
		 * Configure Optotrak Collection
		 * Once the system strobers have been enabled, and all settings are
		 * loaded, the application can set up the Optotrak collection
		 */
		fprintf( stdout, "...OptotrakSetupCollection\n" );
		fprintf( stdout, ".....%d, %.2f, %.0f, %d, %d, %d, %.2f, %.2f, %.0f, %.0f\n",
									 dtSettings.nMarkers,
									 dtSettings.fFrameFrequency,
									 dtSettings.fMarkerFrequency,
									 dtSettings.nThreshold,
									 dtSettings.nMinimumGain,
									 dtSettings.nStreamData,
									 dtSettings.fDutyCycle,
									 dtSettings.fVoltage,
									 dtSettings.fCollectionTime,
									 dtSettings.fPreTriggerTime );
		if( OptotrakSetupCollection( dtSettings.nMarkers,
									 dtSettings.fFrameFrequency,
									 dtSettings.fMarkerFrequency,
									 dtSettings.nThreshold,
									 dtSettings.nMinimumGain,
									 dtSettings.nStreamData,
									 dtSettings.fDutyCycle,
									 dtSettings.fVoltage,
									 dtSettings.fCollectionTime,
									 dtSettings.fPreTriggerTime,
									 OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG | OPTOTRAK_SWITCH_AND_CONFIG_FLAG ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		/*
		 * Wait one second to let the camera adjust.
		 */
		sleep( 1 );

		/*
		 * Prepare for realtime data retrieval.
		 * Activate markers. Turn on the markers prior to data retrieval.
		 */
		fprintf( stdout, "...OptotrakActivateMarkers\n" );
		if( OptotrakActivateMarkers( ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */
		sleep( 1 );

		/*
		 * loop around until a device is plugged or unplugged
		 */
		while( 1 )
		{
			fprintf( stdout, "\nCHANGE THE STROBER CONFIGURATION TO END THIS TEST\n\n" );
			sleep( 1 );

			if( DataGetLatest3D( &uFrameNumber, &uElements, &uFlags, p3dData ) != OPTO_NO_ERROR_CODE )
			{
				goto ERROR_EXIT;
			} /* if */

			/*
			 * display the 3d data
			 */
			fprintf( stdout, "3D Data Display (%d Markers)\n", dtSettings.nMarkers );
			/*
			 * Print out the data.
			 */
			fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
			fprintf( stdout, "Elements    : %8u\n", uElements );
			fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
			for( nCurMarker = 0; nCurMarker < dtSettings.nMarkers; nCurMarker++ )
			{
				DisplayMarker( nCurMarker + 1, p3dData[nCurMarker] );
			} /* for */

			/*
			 * check if the tool configuration has changed
			 */
			if( uFlags & OPTO_TOOL_CONFIG_CHANGED_FLAG )
			{
				fprintf( stdout, "\n\n\n...Tool Configuration Changed\n" );
				sleep( 3 );
				break;
			} /* if */
		} /* while */

		sleep( 1 );

		/*
		 * stop the collection since we are finished at this point
		 */
		fprintf( stdout, "...OptotrakStopCollection\n\n\n" );
		if( OptotrakStopCollection( ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

	} /* for */

	/*
	 * Exiting the loop above means that the tool configuration has changed
	 */
	fprintf( stdout, "...DetermineStroberConfiguration\n" );
	if( DetermineStroberConfiguration( &pdtDeviceHandles, &pdtDeviceHandlesInfo, &nDevices ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */


PROGRAM_COMPLETE:
    /*
     * CLEANUP
     */
	fprintf( stdout, "\n" );
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    OptotrakDeActivateMarkers( );
    TransputerShutdownSystem( );

	/*
	 * free all memory
	 */
	if( pdtDeviceHandlesInfo )
	{
		for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
		{
			AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[nCurDevice].grProperties), 0 );
		} /* for */
	} /* if */
	AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
	AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );

	exit( 0 );


ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString, MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
	OptotrakDeActivateMarkers( );
	TransputerShutdownSystem( );

	/*
	 * free all memory
	 */
	if( pdtDeviceHandlesInfo )
	{
		for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
		{
			AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[nCurDevice].grProperties), 0 );
		} /* for */
	} /* if */
	AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
	AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );

    exit( 1 );

} /* main */
예제 #14
0
void main( int argc, unsigned char *argv[] )
{
	OptotrakSettings
		dtSettings;
    char
        szNDErrorString[MAX_ERROR_STRING_LENGTH + 1];
    int
		i,
		nCurDevice,
		nCurMarker,
		nMarkersToActivate,
		nDevices,
		nDeviceMarkers,
		nNumSensors,
		nCurSensor;
	ApplicationDeviceInformation
		*pdtDevices;
    DeviceHandle
        *pdtDeviceHandles;
    DeviceHandleInfo
        *pdtDeviceHandlesInfo;
	unsigned int
		uFlags,
		uElements,
		uFrameNumber;
    Position3d
        *p3dData;
	float
		*pfCurSensor,
		*pCentroidData;

    /*
     * initialization
	 * intialize variables
     */
	pdtDevices = NULL;
	pdtDeviceHandles = NULL;
	pdtDeviceHandlesInfo = NULL;
	p3dData = NULL;
	nMarkersToActivate = 0;
	nDevices = 0;
	nDeviceMarkers = 0;
	dtSettings.nMarkers = 0;
	dtSettings.fFrameFrequency = SAMPLE_FRAMEFREQ;
	dtSettings.fMarkerFrequency = SAMPLE_MARKERFREQ;
	dtSettings.nThreshold = 30;
	dtSettings.nMinimumGain = 160;
	dtSettings.nStreamData = SAMPLE_STREAMDATA;
	dtSettings.fDutyCycle = SAMPLE_DUTYCYCLE;
	dtSettings.fVoltage = SAMPLE_VOLTAGE;
	dtSettings.fCollectionTime = 1.0;
	dtSettings.fPreTriggerTime = 0.0;

	/*
	 * Announce that the program has started
	 */
	fprintf( stdout, "\nOptotrak Certus sample program #16\n\n" );

	/*
	 * look for the -nodld parameter that indicates 'no download'
	 */
	if( ( argc < 2 ) || ( strncmp( argv[1], "-nodld", 6 ) != 0 ) )
	{
		/*
		 * Load the system of processors.
		 */
		fprintf( stdout, "...TransputerLoadSystem\n" );
		if( TransputerLoadSystem( "system" ) != OPTO_NO_ERROR_CODE )
		{
			goto ERROR_EXIT;
		} /* if */

		sleep( 1 );
	} /* if */

    /*
     * Communication Initialization
     * Once the system processors have been loaded, the application
     * prepares for communication by initializing the system processors.
     */
	fprintf( stdout, "...TransputerInitializeSystem\n" );
    if( TransputerInitializeSystem( OPTO_LOG_ERRORS_FLAG | OPTO_LOG_MESSAGES_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * Determine if this sample will run with the system attached.
	 * This sample is intended for Certus systems.
	 */
	fprintf( stdout, "...DetermineSystem\n" );
	if( uDetermineSystem( ) != OPTOTRAK_CERTUS_FLAG )
	{
		goto PROGRAM_COMPLETE;
	} /* if */

    /*
     * Strober Initialization
     * Once communication has been initialized, the application must
     * determine the strober configuration.
     * The application retrieves device handles and all strober
     * properties from the system.
     */
	fprintf( stdout, "...DetermineStroberConfiguration\n" );
	if( DetermineStroberConfiguration( &pdtDeviceHandles, &pdtDeviceHandlesInfo, &nDevices ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * check if any devices have been detected by the system
	 */
	if( nDevices == 0 )
	{
		fprintf( stdout, ".........no devices detected.  Quitting program...\n" );
		goto PROGRAM_COMPLETE;
	} /* if */

    /*
     * Now that all the device handles have been completely set up,
     * the application can store all the device handle information in
     * an internal data structure.  This will facilitate lookups when
     * a property setting needs to be checked.
     */
    ApplicationStoreDeviceProperties( &pdtDevices, pdtDeviceHandlesInfo, nDevices );

	/*
	 * Change the number of markers to fire for all devices
	 */
	for( nCurDevice = 0; nCurDevice < nDevices; nCurDevice++ )
	{
		nMarkersToActivate = pdtDevices[nCurDevice].b3020Capability?
							   CERTUS_SAMPLE_3020_STROBER_MARKERSTOFIRE : CERTUS_SAMPLE_STROBER_MARKERSTOFIRE;

		SetMarkersToActivateForDevice( &(pdtDevices[nCurDevice]), pdtDeviceHandlesInfo[nCurDevice].pdtHandle->nID, nMarkersToActivate );
	} /* if */
	fprintf( stdout, "\n" );

	/*
	 * Determine the collection settings based on the device properties
	 */
	ApplicationDetermineCollectionParameters( nDevices, pdtDevices, &dtSettings );


    /*
     * Set optional processing flags (this overides the settings in Optotrak.INI).
     */
	fprintf( stdout, "...OptotrakSetProcessingFlags\n" );
    if( OptotrakSetProcessingFlags( OPTO_LIB_POLL_REAL_DATA |
                                    OPTO_CONVERT_ON_HOST |
                                    OPTO_RIGID_ON_HOST ) )
    {
        goto ERROR_EXIT;
    } /* if */

    /*
	 * Load camera parameters.
     */
	fprintf( stdout, "...OptotrakLoadCameraParameters\n" );
    if( OptotrakLoadCameraParameters( "standard" ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * allocate memory for all data
	 */
	if( OptotrakGetStatus( &nNumSensors, NULL, NULL, NULL,
							NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * Ensure that we are firing some markers
	 */
	if( dtSettings.nMarkers == 0 )
	{
		fprintf( stdout, "Error: There are no markers to be activated.\n" );
		goto ERROR_EXIT;
	} /* if */

	p3dData = (Position3d*)malloc( dtSettings.nMarkers * sizeof( Position3d ) );
	pCentroidData = (float*)malloc( dtSettings.nMarkers * nNumSensors * sizeof( float ) );

    /*
     * Configure Optotrak Collection
     * Once the system strobers have been enabled, and all settings are
     * loaded, the application can set up the Optotrak collection
     */
	fprintf( stdout, "...OptotrakSetupCollection\n" );
	fprintf( stdout, ".....%d, %.2f, %.0f, %d, %d, %d, %.2f, %.2f, %.0f, %.0f\n",
								 dtSettings.nMarkers,
			                     dtSettings.fFrameFrequency,
				                 dtSettings.fMarkerFrequency,
					             dtSettings.nThreshold,
						         dtSettings.nMinimumGain,
							     dtSettings.nStreamData,
								 dtSettings.fDutyCycle,
								 dtSettings.fVoltage,
								 dtSettings.fCollectionTime,
								 dtSettings.fPreTriggerTime );
    if( OptotrakSetupCollection( dtSettings.nMarkers,
			                     dtSettings.fFrameFrequency,
				                 dtSettings.fMarkerFrequency,
					             dtSettings.nThreshold,
						         dtSettings.nMinimumGain,
							     dtSettings.nStreamData,
								 dtSettings.fDutyCycle,
								 dtSettings.fVoltage,
								 dtSettings.fCollectionTime,
								 dtSettings.fPreTriggerTime,
								 OPTOTRAK_NO_FIRE_MARKERS_FLAG | OPTOTRAK_BUFFER_RAW_FLAG | OPTOTRAK_SWITCH_AND_CONFIG_FLAG ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

    /*
     * Wait one second to let the camera adjust.
     */
    sleep( 1 );

    /*
     * Prepare for realtime data retrieval.
     * Activate markers. Turn on the markers prior to data retrieval.
     */
	fprintf( stdout, "...OptotrakActivateMarkers\n" );
    if( OptotrakActivateMarkers( ) != OPTO_NO_ERROR_CODE )
    {
        goto ERROR_EXIT;
    } /* if */
	sleep( 1 );

    fprintf( stdout, "\n\nSample Program Results:\n\n" );
	/*
	 * Retrieve a frame of Centroid data.
	 * Use the non-block 'RequestNext'/'ReceiveLatest' call to
	 * retrieve a frame of data.
	 * Add the data retrieval functions return status information in
	 * the uFlags field.
     * All the data retrieval functions return status information in
     * the uFlags field.
     * If the flags indicate that the system configuration has changed,
     * all processing should stop and the application should re-query
     * the system for all device information.
     * NOTE: The re-initialization is not done here in order to
     *       simplify the sample program.
	 */
	if( RequestNextCentroid( ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	while( !DataIsReady( ) );

	if( DataReceiveLatestCentroid( &uFrameNumber, &uElements, &uFlags, pCentroidData ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * display the Centroid data
	 */
    fprintf( stdout, "Centroid Data Display (%d Markers)\n", dtSettings.nMarkers );
    /*
     * Print out the data.
     */
    fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
    fprintf( stdout, "Elements    : %8u\n", uElements );
    fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
	for( nCurMarker = 0, pfCurSensor = pCentroidData; nCurMarker < dtSettings.nMarkers; nCurMarker++ )
	{
		fprintf( stdout, "Marker_%.3d:\n", nCurMarker + 1 );
		for( nCurSensor = 0; nCurSensor < nNumSensors; nCurSensor++, pfCurSensor++ )
		{
			fprintf( stdout, "  sensor_%.2d: %10.4f\n", nCurSensor, *pfCurSensor );
		} /* for */
	} /* for */
	fprintf( stdout, "\n\n" );


    /*
     * Retrieve a frame of 3D data.
     * Use the blocking 'GetLatest' call to retrieve a frame of data.
     */
    if( DataGetLatest3D( &uFrameNumber, &uElements, &uFlags, p3dData ) != OPTO_NO_ERROR_CODE )
	{
		goto ERROR_EXIT;
	} /* if */

	/*
	 * display the 3d data
	 */
    fprintf( stdout, "3D Data Display (%d Markers)\n", dtSettings.nMarkers );
    /*
     * Print out the data.
     */
    fprintf( stdout, "Frame Number: %8u\n", uFrameNumber );
    fprintf( stdout, "Elements    : %8u\n", uElements );
    fprintf( stdout, "Flags       : 0x%04x\n", uFlags );
	for( nCurMarker = 0; nCurMarker < dtSettings.nMarkers; nCurMarker++ )
	{
		DisplayMarker( nCurMarker, p3dData[nCurMarker] );
	} /* for */

    /*
     * De-activate the markers.
     */
	fprintf( stdout, "...OptotrakDeActivateMarkers\n" );
    if( OptotrakDeActivateMarkers() )
    {
        goto ERROR_EXIT;
    } /* if */


PROGRAM_COMPLETE:
    /*
     * CLEANUP
     */
	fprintf( stdout, "\n" );
	fprintf( stdout, "...TransputerShutdownSystem\n" );
    TransputerShutdownSystem( );

	/*
	 * free all memory
	 */
	if( pdtDeviceHandlesInfo )
	{
		for( i = 0; i < nDevices; i++ )
		{
			AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[i].grProperties), 0 );
		} /* for */
	} /* if */
	AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
	AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );
	free( p3dData );
	free( pCentroidData );

	exit( 0 );

ERROR_EXIT:
	/*
	 * Indicate that an error has occurred
	 */
	fprintf( stdout, "\nAn error has occurred during execution of the program.\n" );
    if( OptotrakGetErrorString( szNDErrorString, MAX_ERROR_STRING_LENGTH + 1 ) == 0 )
    {
        fprintf( stdout, szNDErrorString );
    } /* if */

	fprintf( stdout, "\n\n...TransputerShutdownSystem\n" );
	OptotrakDeActivateMarkers( );
	TransputerShutdownSystem( );

	/*
	 * free all memory
	 */
	if( pdtDeviceHandlesInfo )
	{
		for( i = 0; i < nDevices; i++ )
		{
			AllocateMemoryDeviceHandleProperties( &(pdtDeviceHandlesInfo[i].grProperties), 0 );
		} /* for */
	} /* if */
	AllocateMemoryDeviceHandles( &pdtDeviceHandles, 0 );
	AllocateMemoryDeviceHandlesInfo( &pdtDeviceHandlesInfo, pdtDeviceHandles, 0 );
	free( p3dData );
	free( pCentroidData );

    exit( 1 );

} /* main */