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 }