bool mitk::MicroBirdTrackingDevice::CloseConnection()
{
SwitchTransmitter(false); // Switch off the transmitter
int errorCode = CloseBIRDSystem(); // Close connection. This function can take a few seconds
// Error checking
if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
HandleError(errorCode);
// Delete configuration
if (m_TransmitterConfig != NULL)
{
delete [] m_TransmitterConfig;
m_TransmitterConfig = NULL;
}
if (m_SensorConfig != NULL)
{
delete [] m_SensorConfig;
m_SensorConfig = NULL;
}
// Change mode and release mutex
this->SetState(Setup);
// Clear error message
this->SetErrorMessage("");
return true;
}
int Srovnani_NUMB() {
if (Symb.type == NUMB) {
int shod = Symb.number;
Symb = readLexem();
return shod;
} else
CompareError(NUMB);
}
void Compare_NUMB(int *h, int line)
{
if (Symb.type == NUMB) {
*h = Symb.number;
Symb = readLexem();
} else
CompareError(NUMB, line);
}
void Compare_IDENT(char *id, int line)
{
if (Symb.type == IDENT) {
strcpy(id, Symb.ident);
Symb = readLexem();
} else
CompareError(IDENT, line);
}
QString CompareSideBySideView::getText(SourceElementDiffOperation *source, const EDiff::KDiff state, const bool isEmpty)
{
if(isEmpty) {
return "" ;
}
QString text;
bool useText = false ;
switch(state) {
default: {
QString msg = tr("Unknown state:%1 in setup diff").arg(state);
Utils::error(msg);
CompareError(msg);
}
break;
case EDiff::ED_ADDED:
useText = true ;
break;
case EDiff::ED_MODIFIED:
useText = true ;
break;
case EDiff::ED_EQUAL:
useText = true ;
break;
case EDiff::ED_DELETED:
useText = true ;
break;
}
if(useText) {
Element *element = source->element();
switch(element->getType()) {
case Element::ET_ELEMENT:
text = QString("<%1>").arg(element->tag());
break;
case Element::ET_PROCESSING_INSTRUCTION:
text = QString("<? %1 %2 ?>").arg(element->getPITarget()).arg(element->getPIData());
break;
case Element::ET_COMMENT:
text = getElidedText(element->text);
text = QString("Comment: %1").arg(text);
break;
case Element::ET_TEXT:
text = getElidedText(element->text);
if(element->isCDATA()) {
text = QString("Text CDATA: [[%1]]").arg(text);
} else {
text = QString("Text: %1").arg(text);
}
break;
default:
break;
}
}
return text ;
}
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;
}
void CompareSideBySideView::setGraphic(QTreeWidgetItem *currentNodeItem, const EDiff::KDiff state, const bool isEmpty)
{
if(isEmpty) {
currentNodeItem->setBackgroundColor(0, _chrome.emptyColor());
return ;
}
bool useIcon = false;
QIcon icon;
QColor bkColor;
switch(state) {
default: {
QString msg = tr("Unknown state:%1 in CompareSideBySideView::setGraphic").arg(state);
Utils::error(msg);
CompareError(msg);
}
break;
case EDiff::ED_ADDED:
/*if( isReference ) {*/
bkColor = _chrome.addedColor();
/*} else {
bkColor = emptyColor();
}*/
useIcon = true;
icon = _chrome.addedIcon() ;
break;
case EDiff::ED_MODIFIED:
bkColor = _chrome.modifiedColor();
useIcon = true;
icon = _chrome.modIcon();
break;
case EDiff::ED_EQUAL:
bkColor = _chrome.equalsColor();
break;
case EDiff::ED_DELETED:
/*if( isReference ) {
bkColor = emptyColor();
} else {*/
bkColor = _chrome.deletedColor();
/*}*/
useIcon = true;
icon = _chrome.deletedIcon() ;
break;
}
currentNodeItem->setBackgroundColor(0, bkColor);
if(useIcon) {
currentNodeItem->setIcon(0, icon);
}
}
void mitk::MicroBirdTrackingDevice::HandleError(int errorCode)
{
char buffer[1024];
char* pBuffer = &buffer[0];
while(!CompareError(errorCode, BIRD_ERROR_SUCCESS))
{
// Print error number on screen
//cout << "MicroBIRD Error Code: " << errorCode << endl;
// Print error message on screen
errorCode = GetErrorText(errorCode, pBuffer, sizeof(buffer), SIMPLE_MESSAGE);
/// @todo : set error message, does it work?
this->SetErrorMessage(buffer);
}
}
void Compare(LexSymbolType s) {
if (Symb.type == s)
Symb = readLexem();
else
CompareError(s);
}
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 mitk::MicroBirdTrackingDevice::TrackTools()
{
if (this->GetState() != Tracking)
return;
/* Frequency configuration */
double updateRate = 1000.0 / m_SystemConfig.measurementRate;
double measurementDuration = 0.0;
// lock the TrackingFinishedMutex to signal that the execution rights
// are now transfered to the tracking thread
MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
// Because m_StopTracking is used by two threads, access has to be guarded
// by a mutex. To minimize thread locking, a local copy is used here
bool localStopTracking;
/* update the local copy of m_StopTracking */
this->m_StopTrackingMutex->Lock();
localStopTracking = this->m_StopTracking;
this->m_StopTrackingMutex->Unlock();
/* Tracking loop */
while ((this->GetState() == Tracking) && (localStopTracking == false))
{
int errorCode;
unsigned int nOfAttachedSensors = 0;
double timeStamp = 0.0;
int toolNumber = 0; // Numbers for attached sensors only
for (int sensorID = 0; sensorID < m_SystemConfig.numberSensors; sensorID++) // for each sensor grep data
{
if (!m_SensorConfig[sensorID].attached)
continue;
// sensor attached so get record
errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
if (CompareError(errorCode, BIRD_ERROR_SUCCESS))
{ // On SUCCESS, parse sensor information
nOfAttachedSensors++;
timeStamp += record.time; // Get timestamp from record
ToolType* tool = GetMicroBirdTool(toolNumber); /// Get tool (current sensor)
if (tool != NULL)
{
tool->SetTrackingError(record.quality); // Set tracking error (quality) from record
mitk::Point3D position;
position[0] = record.x;
position[1] = record.y;
position[2] = record.z;
tool->SetPosition(position); // Set position
mitk::Quaternion orientation(record.q[1], record.q[2], record.q[3],record.q[0]);
tool->SetOrientation(orientation); // Set orientation as quaternion \todo : verify quaternion order q(r,x,y,z)
tool->SetDataValid(true); // Set data state to valid
}
toolNumber++; // Increment tool number
}
else
{ // ERROR while reading sensor information
HandleError(errorCode);
}
}
/// @todo : is there any synchronisation?
// Average timestamp: timeStamp/nOfAttachedSensors
// Compute sleep time
double sleepTime = updateRate - measurementDuration;
// Sleep
if (sleepTime > 0.0 && sleepTime < 500.0)
{
// Note: we only have to approximately sleep one measurement cycle,
// since the tracker keeps track of the measurement rate itself
itksys::SystemTools::Delay(sleepTime)
//Sleep(static_cast<DWORD>(sleepTime));
}
// Update the local copy of m_StopTracking
this->m_StopTrackingMutex->Lock();
localStopTracking = m_StopTracking;
this->m_StopTrackingMutex->Unlock();
}
// @bug (#1813) : maybe we need to check for localStopTracking=true here?
// m_StopTracking should only ever be updated by StopTracking(), so
// maybe we should not unlock a mutex that nobody is waiting for?
return; // returning from this function (and ThreadStartTracking()) this will end the thread
}
void Compare(LexSymbolType s, int line) {
if (Symb.type == s)
Symb = readLexem();
else
CompareError(s, line);
}
