void DR::EMHandler::disconnect()
{
int errorCode;
m_lock.lock();
if (!m_connected) {
m_lock.unlock();
return;
}
m_running = false;
pthread_join(m_thread, NULL);
m_connected = false;
// Turn off the transmitter before exiting
// We turn off the transmitter by "selecting" a transmitter with an id of "-1"
//
int id = -1;
errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
if (errorCode != BIRD_ERROR_SUCCESS) errorHandler(errorCode);
// Free memory allocations before exiting
delete[] pSensor;
delete[] pXmtr;
m_lock.unlock();
}
bool mitk::MicroBirdTrackingDevice::SwitchTransmitter(bool switchOn)
{
if (switchOn)
{
/* Search for the first attached transmitter and turn it on */
for (short id = 0; id < m_SystemConfig.numberTransmitters; id++)
{
if (m_TransmitterConfig[id].attached)
{
// Transmitter selection is a system function.
// Using the SELECT_TRANSMITTER parameter we send the id of the
// transmitter that we want to run with the SetSystemParameter() call
int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
else
return true; //break; // \TODO: Stop after the first attached transmitter was turned off?
}
}
}
else
{
/* Transmitter selection is a system function, Note: a selector of -1 switches off the current transmitter */
short TRANSMITTER_OFF = -1;
int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &TRANSMITTER_OFF, sizeof(TRANSMITTER_OFF));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
else
return true;
}
// Return success
return true;
}
bool mitk::MicroBirdTrackingDevice::OpenConnection()
{
/* Check whether in setup mode */
if (this->GetState() != Setup)
{
this->SetErrorMessage("Can only try to open the connection if in setup mode");
return false;
}
int errorCode; // Holds error code
/* Initialize the PCIBIRD driver and DLL */
errorCode = InitializeBIRDSystem(); // this function can take a few seconds
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
/// @todo : check for transmitter serial numbers here?
// Serial numbers could be compared to known ones for some simple
// parameters sanity check (measurement frequency etc.)
/* Get system configuration */
errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
/* use metric measurements in mm */
errorCode = SetSystemParameter(METRIC, &m_metric, sizeof(m_metric));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
/* Set the measurement rate to m_measurementRate */
if ((m_measurementRate > 30) && (m_measurementRate < 80))
{
errorCode = SetSystemParameter(MEASUREMENT_RATE, &m_measurementRate, sizeof(m_measurementRate));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
}
/* Set power line frequency */
if ((m_pl >= 50) && (m_pl <= 60))
{
errorCode = SetSystemParameter(POWER_LINE_FREQUENCY, &m_pl, sizeof(m_pl));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
}
/* Set AGC mode */
m_agc = m_agcModeBoth ? TRANSMITTER_AND_SENSOR_AGC : SENSOR_AGC_ONLY;
errorCode = SetSystemParameter(AGC_MODE, &m_agc, sizeof(m_agc));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
/* Get system configuration */
errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
return false;
}
/* Get sensor information */
m_SensorConfig = new SENSOR_CONFIGURATION[m_SystemConfig.numberSensors];
for (int i = 0; i < m_SystemConfig.numberSensors; i++)
{
errorCode = GetSensorConfiguration(i, &(m_SensorConfig[i]));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
}
/* Initialize the quality parameter structure */
QUALITY_PARAMETERS qualityParameters; // = { 164, 0, 32, 3072 };
GetSensorParameter(i, QUALITY, &qualityParameters, sizeof(qualityParameters));
/* Set data format to matrix format */
//DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_MATRIX_TIME_Q;
/* Set data format to quaternion format */
DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_QUATERNION_TIME_Q;
/* Set data format for sensor */
DATA_FORMAT_TYPE *pTempBuffer = &tempBuffer;
errorCode = SetSensorParameter(i, DATA_FORMAT, pTempBuffer, sizeof(tempBuffer));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
}
}
/* Initialize tools vector */
{
MutexLockHolder(*m_ToolsMutex);
for (int i = 0; i < m_SystemConfig.numberSensors; i++)
{
if (m_SensorConfig[i].attached)
m_Tools.push_back(ToolType::New());
}
}
/* Get transmitter configuration */
m_TransmitterConfig = new TRANSMITTER_CONFIGURATION[m_SystemConfig.numberTransmitters];
for (int i = 0; i < m_SystemConfig.numberTransmitters; i++)
{
errorCode = GetTransmitterConfiguration(i, &(m_TransmitterConfig[i]));
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
HandleError(errorCode);
}
}
/* Switch off transmitter */
SwitchTransmitter(true);
SwitchTransmitter(false);
// @todo : set up error scaling?
/* finish - now all tools should be added, initialized and enabled, so that tracking can be started */
this->SetState(Ready);
this->SetErrorMessage("");
return true; // Return success
}
void* DR::EMHandler::thread_connect()
{
int errorCode;
m_lock.lock();
// Initialize the ATC3DG driver and DLL
//
// It is always necessary to first initialize the ATC3DG "system". By
// "system" we mean the set of ATC3DG cards installed in the PC. All cards
// will be initialized by a single call to InitializeBIRDSystem(). This
// call will first invoke a hardware reset of each board. If at any time
// during operation of the system an unrecoverable error occurs then the
// first course of action should be to attempt to Recall InitializeBIRDSystem()
// if this doesn't restore normal operating conditions there is probably a
// permanent failure - contact tech support.
// A call to InitializeBIRDSystem() does not return any information.
//
errorCode = InitializeBIRDSystem();
if (errorCode != BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
m_lock.unlock();
return 0;
}
BOOL metric = true;
errorCode = SetSystemParameter(METRIC, &metric, sizeof(metric));
if (errorCode != BIRD_ERROR_SUCCESS) errorHandler(errorCode);
// GET SYSTEM CONFIGURATION
//
// In order to get information about the system we have to make a call to
// GetBIRDSystemConfiguration(). This call will fill a fixed size structure
// containing amongst other things the number of boards detected and the
// number of sensors and transmitters the system can support (Note: This
// does not mean that all sensors and transmitters that can be supported
// are physically attached)
//
errorCode = GetBIRDSystemConfiguration(&ATC3DG.m_config);
if (errorCode != BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
m_lock.unlock();
return 0;
}
// GET SENSOR CONFIGURATION
//
// Having determined how many sensors can be supported we can dynamically
// allocate storage for the information about each sensor.
// This information is acquired through a call to GetSensorConfiguration()
// This call will fill a fixed size structure containing amongst other things
// a status which indicates whether a physical sensor is attached to this
// sensor port or not.
//
pSensor = new CSensor[ATC3DG.m_config.numberSensors];
for (int i = 0; i < ATC3DG.m_config.numberSensors; i++)
{
errorCode = GetSensorConfiguration(i, &(pSensor + i)->m_config);
if (errorCode != BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
m_lock.unlock();
return 0;
}
}
// GET TRANSMITTER CONFIGURATION
//
// The call to GetTransmitterConfiguration() performs a similar task to the
// GetSensorConfiguration() call. It also returns a status in the filled
// structure which indicates whether a transmitter is attached to this
// port or not. In a single transmitter system it is only necessary to
// find where that transmitter is in order to turn it on and use it.
//
pXmtr = new CXmtr[ATC3DG.m_config.numberTransmitters];
for (int i = 0; i < ATC3DG.m_config.numberTransmitters; i++)
{
errorCode = GetTransmitterConfiguration(i, &(pXmtr + i)->m_config);
if (errorCode != BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
m_lock.unlock();
return 0;
}
}
// Search for the first attached transmitter and turn it on
//
for (short id = 0; id < ATC3DG.m_config.numberTransmitters; id++)
{
if ((pXmtr + id)->m_config.attached)
{
// Transmitter selection is a system function.
// Using the SELECT_TRANSMITTER parameter we send the id of the
// transmitter that we want to run with the SetSystemParameter() call
errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
if (errorCode != BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
m_lock.unlock();
return 0;
}
break;
}
}
printf("Seting Sensor Parameter\n");
DATA_FORMAT_TYPE format = DOUBLE_POSITION_QUATERNION;
// scan the sensors and request a record
for (int sensorID = 0; sensorID < ATC3DG.m_config.numberSensors; sensorID++)
{
errorCode = SetSensorParameter(
sensorID, // index number of target sensor
DATA_FORMAT, // command parameter type
&format, // address of data source buffer
sizeof(format) // size of source buffer
);
if (errorCode != BIRD_ERROR_SUCCESS) errorHandler(errorCode);
}
m_connected = true;
if (m_connected) printf("[EM]:Connected\n");
m_lock.unlock();
return 0;
}
int main2()
{
//Tracker tracker(10000);
CSystem PCIBird;
CSensor *pSensor;
CXmtr *pXmtr;
int errorCode;
int i;
int sensorID;
short id;
int records = 10000;
double rate = 255.0f;
printf("\n\n");
printf("ATC3DG Timing Test Application\n");
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Initialize the PCIBIRD driver and DLL
//
// It is always necessary to first initialize the PCIBird "system". By
// "system" we mean the set of PCIBird cards installed in the PC. All cards
// will be initialized by a single call to InitializeBIRDSystem(). This
// call will first invoke a hardware reset of each board. If at any time
// during operation of the system an unrecoverable error occurs then the
// first course of action should be to attempt to Recall InitializeBIRDSystem()
// if this doesn't restore normal operating conditions there is probably a
// permanent failure - contact tech support.
// A call to InitializeBIRDSystem() does not return any information.
//
printf("Initializing ATC3DG system...\n");
errorCode = InitializeBIRDSystem();
if(errorCode!=BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
exit(1);
}
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// GET SYSTEM CONFIGURATION
//
// In order to get information about the system we have to make a call to
// GetBIRDSystemConfiguration(). This call will fill a fixed size structure
// containing amongst other things the number of boards detected and the
// number of sensors and transmitters the system can support (Note: This
// does not mean that all sensors and transmitters that can be supported
// are physically attached)
//
errorCode = GetBIRDSystemConfiguration(&PCIBird.m_config);
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// GET SENSOR CONFIGURATION
//
// Having determined how many sensors can be supported we can dynamically
// allocate storage for the information about each sensor.
// This information is acquired through a call to GetSensorConfiguration()
// This call will fill a fixed size structure containing amongst other things
// a status which indicates whether a physical sensor is attached to this
// sensor port or not.
//
pSensor = new CSensor[PCIBird.m_config.numberSensors];
for(i=0;i<PCIBird.m_config.numberSensors;i++)
{
errorCode = GetSensorConfiguration(i, &(pSensor+i)->m_config);
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
}
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// GET TRANSMITTER CONFIGURATION
//
// The call to GetTransmitterConfiguration() performs a similar task to the
// GetSensorConfiguration() call. It also returns a status in the filled
// structure which indicates whether a transmitter is attached to this
// port or not. In a single transmitter system it is only necessary to
// find where that transmitter is in order to turn it on and use it.
//
pXmtr = new CXmtr[PCIBird.m_config.numberTransmitters];
for(i=0;i<PCIBird.m_config.numberTransmitters;i++)
{
errorCode = GetTransmitterConfiguration(i, &(pXmtr+i)->m_config);
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
}
//
// Set the measurement rate
//
errorCode = SetSystemParameter(MEASUREMENT_RATE, &rate, sizeof(rate));
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Search for the first attached transmitter and turn it on
//
for(id=0;id<PCIBird.m_config.numberTransmitters;id++)
{
if((pXmtr+id)->m_config.attached)
{
// Transmitter selection is a system function.
// Using the SELECT_TRANSMITTER parameter we send the id of the
// transmitter that we want to run with the SetSystemParameter() call
errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
break;
}
}
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Collect data from all birds
// Loop through all sensors and get a data record if the sensor is attached.
// Print result to screen
// Note: The default data format is DOUBLE_POSITION_ANGLES. We can use this
// format without first setting it.
//
//
DOUBLE_POSITION_ANGLES_TIME_Q_RECORD record[8*4], *pRecord = record;
// Set the data format type for each attached sensor.
for(i=0;i<PCIBird.m_config.numberSensors;i++)
{
DATA_FORMAT_TYPE type = DOUBLE_POSITION_ANGLES_TIME_Q;
errorCode = SetSensorParameter(i,DATA_FORMAT,&type,sizeof(type));
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
}
// collect as many records as specified in the records var
printf("Collecting %d data records at %3.2fhz...\n", records, rate*3.0);
for(i=0;i<records;i++)
{
errorCode = GetSynchronousRecord(ALL_SENSORS, pRecord, sizeof(record[0]) * PCIBird.m_config.numberSensors);
if(errorCode!=BIRD_ERROR_SUCCESS)
{
errorHandler(errorCode);
}
// Get a snap shot of the PC's time
time_t currentTime;
time(¤tTime);
// Some user IO, every 500 records print a progress indication
if( !(i % 500))
printf( ".");
// scan the sensors and request a record if the sensor is physically attached
for(sensorID=0;sensorID<PCIBird.m_config.numberSensors;sensorID++)
{
// get the status of the last data record
// only report the data if everything is okay
unsigned int status = GetSensorStatus( sensorID);
if( status == VALID_STATUS)
{
// save output to buffer
sprintf(output[i][sensorID], "[%d] 0x%04x %8.3f %8.3f %8.3f: %8.2f %8.2f %8.2f %8.4f\n",
sensorID,
status,
record[sensorID].x,
record[sensorID].y,
record[sensorID].z,
record[sensorID].a,
record[sensorID].e,
record[sensorID].r,
record[sensorID].time
);
}
}
}
printf("\nSaving results to 'GetSynchronousRecord.dat'..\n");
FILE *fp = fopen( "GetSynchronousRecord.dat", "w");
for(i=0;i<records;i++)
for(sensorID=0;sensorID<PCIBird.m_config.numberSensors;sensorID++)
fprintf( fp, output[i][sensorID]);
fclose( fp);
printf("Complete...\n");
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Turn off the transmitter before exiting
// We turn off the transmitter by "selecting" a transmitter with an id of "-1"
//
id = -1;
errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode);
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Free memory allocations before exiting
//
delete[] pSensor;
delete[] pXmtr;
return 0;
}
