void DR::EMHandler::getDynamicRecording(pugi::xml_node &frame)
{
int errorCode;
if (!m_connected)
return;
pugi::xml_node trackSTAR = frame.append_child("TrackSTAR");
trackSTAR.append_attribute("Unit").set_value("millimeter");
m_lock.lock();
// 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_QUATERNION_RECORD record, *pRecord = &record;
// scan the sensors and request a record
for (int sensorID = 0; sensorID < ATC3DG.m_config.numberSensors; sensorID++)
{
// sensor attached so get record
errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
if (errorCode != BIRD_ERROR_SUCCESS) {
errorHandler(errorCode);
return;
}
// 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)
{
pugi::xml_node bird_node = trackSTAR.append_child("BIRD");
pugi::xml_attribute id = bird_node.append_attribute("id");
id.set_value(sensorID);
pugi::xml_node position_node = bird_node.append_child();
position_node.set_name("Position");
position_node.append_attribute("X").set_value(record.x*1000.0f);
position_node.append_attribute("Y").set_value(record.y*1000.0f);
position_node.append_attribute("Z").set_value(record.z*1000.0f);
pugi::xml_node rotation_node = bird_node.append_child();
rotation_node.set_name("Rotation");
rotation_node.append_attribute("W").set_value(record.q[0]);
rotation_node.append_attribute("X").set_value(record.q[1]);
rotation_node.append_attribute("Y").set_value(record.q[2]);
rotation_node.append_attribute("Z").set_value(record.q[3]);
}
}
m_lock.unlock();
}
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
}
