//==============================================================================
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;
}
