//============================================================================== int COptoTrack::SetSensorsTransformIdx( int nSensors, int * piTransformIdx ){ int i, iTrans; if( nSensors < m_nSensorsToWrite ){ CP_printf("\nError: wrong input number of sensors: %d", nSensors); CP_printf(" Should be: %d\n", m_nSensorsToWrite); return 1; } // we allow input longer than actual m_nSensorsToWrite // it's a hack to set a single sensor transform idx // Matlab will not send a single number as a SAFEARRAY if( nSensors > m_nSensorsToWrite ){ CP_printf("\nWarning: wrong input number of sensors: %d", nSensors); CP_printf(" Using first %d values.\n", m_nSensorsToWrite); } for( i = 0; i < m_nSensorsToWrite; i++ ){ iTrans = piTransformIdx[i]; // Check transform for valid range if( iTrans < 0 || iTrans >= N_TRANSFORMS ){ CP_printf("\nCannot set transform. Transfrom index out of range: %d\n", iTrans ); return 1; } } for( i = 0; i < m_nSensorsToWrite; i++ ){ m_atSensorState[i].iTransformIdx = piTransformIdx[i]; } return 0; }
//=========================================================================== int CSerialThread::m_InitSerialThread( ){ int iRet; if( m_hSerialThread ) { CP_printf("Error in m_InitSerialThread()! Thread is already running.\n"); return 1; } // Init sliding buffer SetBuffer(sizeof(IScanData), ISCAN_BUFFER_SIZE); CP_printf("Connecting to %s port\n", x_aszComNames[ m_iComPortIdx-1]); iRet = m_Serial.connect(x_aszComNames[ m_iComPortIdx-1], m_iBaudRate, m_iSerialParity); if(iRet) { CP_printf("Error in m_InitSerialThread, m_Serial.connect() failed\n"); return 1; } ResetTimeAndBuffer(); m_hSerialThread = CreateThread( NULL, // pointer to security attributes 0, // initial thread stack size m_SerialThread, // pointer to thread function (LPVOID) this, // argument for new thread 0, // no specific creation flags &m_dwThreadId ); // pointer to receive thread ID if( !m_hSerialThread ) { CP_printf("Error in m_InitSerialThread()! CreateThread() failed.\n"); return 1; } return 0; }
//============================================================================== int COptoTrack::DisconnectOpto(){ int iSensor; if( !IsRunning() ) { CP_printf("Opto is not connected!\n"); return 1; } CP_printf("\nDisonnecting OptoTrak... "); // Stop the thread, specify stopping time in ms (duration of 10 frames) if( m_StopReadingThread( int(FRAME_TIME_MS) * 10 ) > 1 ) { CP_printf("\nError: m_StopReadingThread() failed!\n"); return 1; } // release the buffers RemoveBuffer(); // Set signal level indicator to OFF for(iSensor =0; iSensor < N_SENSORS_MAX; iSensor++) m_iDistirtionLevel[iSensor]= ecSensorOff; CP_printf("Opto disconnected.\n"); CP_printf("Collected %d frames.\n", m_iReadCount); CP_printf("Last frame index from OptotTrak: %d\n", m_iOptoFrameIdx); CP_printf("Number of missed frames: %d\n", m_nMissedFrames); CP_printf("Number of missed times: %d\n", m_nMissedTimes); CP_printf("++++++++++++++++++++++++++++++++++++++\n"); return 0; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::GetSFreq(float * pfSamplingRate ){ HRESULT hRes; hRes = g_pRPcoX->raw_GetSFreq( pfSamplingRate ); if( hRes != S_OK ){ CP_printf("GetSFreq() ActiveX error!\n"); } CP_printf("GetSFreq(): %f \n", * pfSamplingRate); return 0; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::GetStatus(long * piStatus){ HRESULT hRes; hRes = g_pRPcoX->raw_GetStatus( piStatus ); if( hRes != S_OK ){ CP_printf("GetStatus() ActiveX error!\n"); return 0; } CP_printf("GetStatus(): %d \n", * piStatus); return 0; }
//============================================================================== // 1D array of ints, dim unspecified, allow Nx1 or 1xN arrays int g_CheckVariantArray( int * pnDim1, VARIANT vMtx, int ** ppiMtx ){ SAFEARRAY * psaMtx; int nDims; long iDim1, iDim2; VARTYPE tSaType; psaMtx = vMtx.parray; *pnDim1 = 0; SafeArrayGetVartype( psaMtx, &tSaType ); if( tSaType != VT_I4 ){ CP_printf("\ng_CheckVariantArray(): wrong array type, expecting array of ints!\n"); return 1; } nDims = SafeArrayGetDim( psaMtx ); if( nDims !=2 && nDims !=1 ) { // Allow Nx1 or 1xN arrays CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting 1xN, or Nx1 array!\n"); return 1; } SafeArrayGetLBound( psaMtx, 1, &iDim1 ); if( iDim1 != 0) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting 1xN, or Nx1 array!\n"); return 1; } SafeArrayGetUBound( psaMtx, 1, &iDim1 ); if( nDims == 2 ) { SafeArrayGetLBound( psaMtx, 2, &iDim2 ); if( iDim2 != 0) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting 1xN, or Nx1 array!\n"); return 1; } SafeArrayGetUBound( psaMtx, 2, &iDim2 ); } else iDim2 = 0; // stupid Matlab hack allowing any 2D array with one redundant dim if( (iDim2 != 0) && (iDim1 != 0)) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting 1xN, or Nx1 array!\n"); return 1; } *pnDim1 = iDim1 + iDim2 + 1; // if we get here, one size is always zero SafeArrayAccessData(psaMtx, (void **) ppiMtx); // Get data pointer return 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; }
//=========================================================================== int CSerialThread::SetComPort( int iPortIdx){ if( m_hSerialThread ) { CP_printf( "Error: Cannot change COM port index while COM is active.\n"); return 1; } if( iPortIdx < 1 || iPortIdx > N_COM_NAMES ){ CP_printf( "Error: Invalid COM port index: %d\n", iPortIdx); return 2; } m_iComPortIdx = iPortIdx; return 0; }
//=========================================================================== int CEyeTrack::SetComPort( int iPortIdx){ if( IsRunning() ) { CP_printf( "Error: Cannot change COM port index while COM is active.\n"); return 1; } if( iPortIdx < 1 || iPortIdx > N_COM_NAMES ){ CP_printf( "Error: Invalid COM port index: %d\n", iPortIdx); return 2; } m_iComPortIdx = iPortIdx; return 0; }
//=========================================================================== int CSerialThread::m_IScanReadLoop(){ int iErr, iItem, iReportSync; int aiData[N_DATA_ITEMS]; iReportSync = 1; // report next sync while(TRUE){ iErr = m_WaitForSync( iReportSync ); if( iErr) goto LoopExit; // exit check for( iItem=0; iItem<N_DATA_ITEMS; iItem++) { iErr = m_ReadDataItem( &aiData[iItem]); if(!m_blnThreadActive) goto LoopExit; // exit check if( iErr) break; } if( iErr ) { iReportSync = 1; // report next sync } else { m_WriteData( aiData, N_DATA_ITEMS ); iReportSync = 0; // don't report next sync } } LoopExit: CP_printf("\n\n"); return 0; }
//============================================================================== int CEyeTrack::CmdStart(){ CP_printf("CEyeTrack::CmdStart()\n"); ConnectISCAN(); return 0; }
//=========================================================================== // wait for 2 sync chars in a row int CSerialThread::m_WaitForSync( int iReportSync ){ char chIn; int nChars, nRead, nTimeouts; nChars = 0; nTimeouts = 0; while( TRUE ) { // wait for the first header byte do { chIn = 0; // in case of time-out, chIn is not changed nRead = m_Serial.getArray( &chIn, 1 ); if(!m_blnThreadActive) return 1; // exit check if( nRead == 0) nTimeouts++; nChars += nRead; } while( chIn != HEADER_CHAR); // get next char and check if it's header chIn = 0; // in case of time-out, chIn is not changed nRead = m_Serial.getArray( &chIn, 1 ); if(!m_blnThreadActive) return 1; // exit check nChars += nRead; if( nRead == 0) nTimeouts++; if( (nRead == 1) && (chIn == HEADER_CHAR)) { if( iReportSync) { CP_printf("\nFound serial sync: %d chars, %d timeouts \n", nChars, nTimeouts); } return 0; // got 2 sync chars in a row } } return 0; // not used }
//============================================================================== int CEyeTrack::CmdStop(){ CP_printf("CEyeTrack::CmdStop()\n"); DisconnectISCAN(); return 0; }
//============================================================================== int CTDT_Stream::SetArmedState(){ int iRet; UINT uiTempID; m_bscSampIdx= m_bscDeviceName + SZ_SAMP_COUNT; m_bscBuff= m_bscDeviceName + SZ_BUF_DATA; m_bscBufIdx = m_bscDeviceName + SZ_BUF_IDX; // do nothing if not in idle state if(m_iState != ecStateIdle) { CP_printf("Cannot set armed state - the system is not in the idle state!\n"); return 1; } // Create file uiTempID = GetTempFileName( (char *)m_bscFilePath, // directory name "TS_", // file name prefix 0, // generate suffix from time m_szTmpFname ); // file name buffer if( uiTempID == 0 ){ CP_printf("Error in SetArmedState, GetTempFileName() failed.\n\ Make sure that the output directory exists:\n %s \n", (char *)m_bscFilePath); return 1; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::ClearCOF( long * piResult ){ HRESULT hRes; hRes = g_pRPcoX->raw_ClearCOF(piResult); if( hRes != S_OK ){ CP_printf("ClearCOF() ActiveX error!\n"); *piResult = 0; } if( *piResult ) CP_printf("ClearCOF() - Ok. \n"); else CP_printf("ClearCOF() failed. \n"); return 0; }
//============================================================================== // 2D array of ints, both dims unspecified int g_CheckVariantArray( int * pnDim1, int * pnDim2, VARIANT vMtx, int ** ppiMtx ){ SAFEARRAY * psaMtx; int nDims; long iDim; VARTYPE tSaType; psaMtx = vMtx.parray; *pnDim1 = 0; *pnDim2 = 0; if( vMtx.vt != (VT_ARRAY|VT_I4)){ CP_printf("\ng_CheckVariantArray(): wrong VARIANT type, expecting array of ints!\n"); return 1; } SafeArrayGetVartype( psaMtx, &tSaType ); if( tSaType != VT_I4 ){ CP_printf("\ng_CheckVariantArray(): wrong array type, expecting array of 4-byte integers!\n"); return 1; } nDims = SafeArrayGetDim( psaMtx ); if( nDims !=2 ) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions\n" ); return 1; } SafeArrayGetLBound( psaMtx, 1, &iDim ); if( iDim != 0) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions!\n" ); return 1; } SafeArrayGetLBound( psaMtx, 2, &iDim ); if( iDim != 0) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions\n" ); return 1; } SafeArrayGetUBound( psaMtx, 1, &iDim ); iDim++; // we return the size, which is upperbound +1 * pnDim1 = iDim; SafeArrayGetUBound( psaMtx, 2, &iDim ); iDim++; // we return the size, which is upperbound +1 * pnDim2 = iDim; SafeArrayAccessData(psaMtx, (void **) ppiMtx); // Get data pointer return 0; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::ReadCOF(BSTR bstFileName, long * piResult ){ HRESULT hRes; hRes = g_pRPcoX->raw_ReadCOF( bstFileName, piResult); if( hRes != S_OK ){ CP_printf("ReadCOF() ActiveX error!\n"); *piResult = 0; } if( *piResult ) CP_printf("ReadCOF() - Ok. \n"); else CP_printf("ReadCOF() failed. \n"); return 0; }
//============================================================================== int CEyeTrack::DisconnectISCAN(){ // HRESULT hRet; m_CloseSerialThread( ); CP_printf("DisconnectISCAN\n"); return 0; }
//=========================================================================== int CReadingThread::m_StopReadingThread() { if( m_hReadingThread == NULL ) { CP_printf("\nError in m_StopReadingThread()! Thread is not running.\n"); return 1; } //Terminate thread m_blnThreadActive = false; Sleep( m_nSleepMs ); // Wait for the thread to finish if( m_hReadingThread != NULL ) { CP_printf("\nError in m_StopReadingThread()! Thread is still running.\n"); return 1; } return 0; }
//============================================================================== int CTimeSync::DisconnectServer(){ HRESULT hRet; // hRet = m_pTDev->raw_CloseConnection( ); CP_printf("Disconnected from TDT server\n"); m_iState = ecInitialized; return 0; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::ConnectRX5(BSTR bstInterfaceType, long iDevIdx, long * piResult){ HRESULT hRes; hRes = g_pRPcoX->raw_ConnectRX5(bstInterfaceType, iDevIdx, piResult); if( hRes != S_OK ){ CP_printf("ConnectRX5() ActiveX error!\n"); *piResult = 0; } if( *piResult ) CP_printf("ConnectRX5() - Ok. \n"); else CP_printf("ConnectRX5() failed. \n"); return 0; }
//============================================================================== int g_StopFrameCounter(){ if( !x_iPortIO_Granted ) return 1; if( !x_iCounterStarted ) return 2; PortOutput( LPT_CONTROL, GATE_OFF); Sleep(20); // enough to finish 60Hz frames CP_printf("Frame counter stopped. Total N frames = %d\n", x_GetFrameCount() ); x_iCounterStarted = 0; return 0; }
//=========================================================================== int CSerialThread::SetBaudRate( int iRate ){ int i; if( m_hSerialThread ) { CP_printf( "Error: Cannot change COM port baud rate while COM is active.\n"); return 1; } // Check if the baud rate is valid for(i=0; i<N_BAUD_RATES; i++) { if ( x_aiStdBaudRates[i] == iRate) goto ExitSuccess; } CP_printf( "Error: Invalid COM port baud rate: %d\n", iRate); return 3; ExitSuccess: m_iBaudRate = iRate; return 0; }
//============================================================================== // 2D array of doubles, second dim unspecified int g_CheckVariantArray( int nDim1, int * pnDim2, VARIANT vMtx, double ** ppdMtx ){ SAFEARRAY * psaMtx; int nDims; long iDim; VARTYPE tSaType; psaMtx = vMtx.parray; *pnDim2 = 0; SafeArrayGetVartype( psaMtx, &tSaType ); if( tSaType != VT_R8 ){ CP_printf("\ng_CheckVariantArray(): wrong array type, expecting array of doubles!\n"); return 1; } nDims = SafeArrayGetDim( psaMtx ); if( nDims !=2 ) { CP_printf("\ng_CheckVariantArray(): wrong number of dimensions, expecting 2D array!\n" ); return 1; } SafeArrayGetLBound( psaMtx, 1, &iDim ); if( iDim != 0) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting %dxN array!\n", nDim1 ); return 1; } SafeArrayGetUBound( psaMtx, 1, &iDim ); if( iDim+1 != nDim1) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting %dxN array!\n", nDim1 ); return 1; } SafeArrayGetLBound( psaMtx, 2, &iDim ); if( iDim != 0) { CP_printf("\ng_CheckVariantArray(): wrong array dimensions, expecting %dxN array!\n", nDim1 ); return 1; } SafeArrayGetUBound( psaMtx, 2, &iDim ); iDim++; // we return the size, which is upperbound +1 * pnDim2 = iDim; SafeArrayAccessData(psaMtx, (void **) ppdMtx); // Get data pointer return 0; }
//============================================================================== int CTimeSync::ConnectServer( ){ HRESULT hRet; long lRes; //TODO: Check for success // hRet = m_pTDev->raw_ConnectServer( m_bscServerName, &lRes ); //TODO: Check for valid device name here // hRet = m_pTDev->raw_GetDeviceStatus( m_bscDeviceName, &lRes ); CP_printf("Connected to TDT server: %s\n", (char *) m_bscServerName); m_iState = ecStateIdle; return 0; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::GetTagVal(BSTR bstTag, float * pfVal ){ HRESULT hRes; hRes = g_pRPcoX->raw_GetTagVal( bstTag, pfVal ); if( hRes != S_OK ){ CP_printf("GetTagVal() ActiveX error!\n"); } return 0; }
//============================================== int g_Test() { HRESULT hRes; _bstr_t bscTmp; long iRes; bscTmp = "GB"; // "USB" or "GB" hRes = g_pRPcoX->raw_ConnectRP2(bscTmp, 1, &iRes); if( hRes != S_OK ) { CP_printf("ConnectRP2() ActiveX error!\n"); return 0; } if( iRes ) CP_printf("ConnectRP2() successful. \n"); else CP_printf("ConnectRP2() failed. \n"); return 0; }
//=========================================================================== // item is a short (2 bytes) int CSerialThread::m_ReadDataItem( int * piItem ){ int nRead; short shtItem; *piItem = 0; nRead = m_Serial.getArray( (char *)&shtItem, sizeof(shtItem) ); if(!m_blnThreadActive) return 1; // exit check if( nRead != sizeof(shtItem)) { CP_printf("\nTimeout in m_ReadDataItem()\n"); return 2; } if(shtItem == HEADER_SHORT) { CP_printf("\nData length mismatch in m_ReadDataItem()\n"); return 3; // actuall number of items is shorter? } *piItem = shtItem; return 0; }
//------------------------------------------------------- HRESULT CP_TimeSyncClass::SetTagVal(BSTR bstTag, float fVal, long * piResult){ HRESULT hRes; hRes = g_pRPcoX->raw_SetTagVal( bstTag, fVal, piResult ); if( hRes != S_OK ){ CP_printf("SetTagVal() ActiveX error!\n"); } return 0; }
//=========================================================================== int COptoTrack::ShutdownFunc( ){ int iErr; // Shutdown the transputer message passing system. iErr = TransputerShutdownSystem(); if( iErr ) { CP_printf("\nTransputerShutdownSystem() Failed!\n"); return 1; } return 0; }