17 #include "mitkMicrobirdTrackingDevice.h"
19 #include <itksys/SystemTools.hxx>
20 #include <itkMutexLockHolder.h>
30 m_measurementRate(68.3),
31 m_TransmitterConfig(NULL),
38 this->
m_Data = mitk::DeviceDataMicroBird;
57 m_MultiThreader->TerminateThread(m_ThreadID);
58 m_MultiThreader = NULL;
60 m_ToolsMutex->Unlock();
64 this->CloseConnection();
66 if (m_TransmitterConfig != NULL)
67 delete [] m_TransmitterConfig;
68 if (m_SensorConfig != NULL)
69 delete [] m_SensorConfig;
78 if (this->GetState() !=
Setup)
80 this->SetErrorMessage(
"Can only try to open the connection if in setup mode");
87 errorCode = InitializeBIRDSystem();
88 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
90 HandleError(errorCode);
98 errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
99 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
101 HandleError(errorCode);
106 errorCode = SetSystemParameter(METRIC, &m_metric,
sizeof(m_metric));
107 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
109 HandleError(errorCode);
114 if ((m_measurementRate > 30) && (m_measurementRate < 80))
116 errorCode = SetSystemParameter(MEASUREMENT_RATE, &m_measurementRate,
sizeof(m_measurementRate));
117 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
119 HandleError(errorCode);
125 if ((m_pl >= 50) && (m_pl <= 60))
127 errorCode = SetSystemParameter(POWER_LINE_FREQUENCY, &m_pl,
sizeof(m_pl));
128 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
130 HandleError(errorCode);
136 m_agc = m_agcModeBoth ? TRANSMITTER_AND_SENSOR_AGC : SENSOR_AGC_ONLY;
137 errorCode = SetSystemParameter(AGC_MODE, &m_agc,
sizeof(m_agc));
138 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
140 HandleError(errorCode);
145 errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
146 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
148 HandleError(errorCode);
153 m_SensorConfig =
new SENSOR_CONFIGURATION[m_SystemConfig.numberSensors];
154 for (
int i = 0; i < m_SystemConfig.numberSensors; i++)
156 errorCode = GetSensorConfiguration(i, &(m_SensorConfig[i]));
157 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
159 HandleError(errorCode);
163 QUALITY_PARAMETERS qualityParameters;
164 GetSensorParameter(i, QUALITY, &qualityParameters,
sizeof(qualityParameters));
169 DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_QUATERNION_TIME_Q;
172 DATA_FORMAT_TYPE *pTempBuffer = &tempBuffer;
173 errorCode = SetSensorParameter(i, DATA_FORMAT, pTempBuffer,
sizeof(tempBuffer));
174 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
176 HandleError(errorCode);
183 for (
int i = 0; i < m_SystemConfig.numberSensors; i++)
185 if (m_SensorConfig[i].attached)
192 m_TransmitterConfig =
new TRANSMITTER_CONFIGURATION[m_SystemConfig.numberTransmitters];
193 for (
int i = 0; i < m_SystemConfig.numberTransmitters; i++)
195 errorCode = GetTransmitterConfiguration(i, &(m_TransmitterConfig[i]));
196 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
198 HandleError(errorCode);
203 SwitchTransmitter(
true);
204 SwitchTransmitter(
false);
209 this->SetState(Ready);
210 this->SetErrorMessage(
"");
220 for (
short id = 0;
id < m_SystemConfig.numberTransmitters;
id++)
222 if (m_TransmitterConfig[
id].attached)
227 int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &
id,
sizeof(
id));
228 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
230 HandleError(errorCode);
241 short TRANSMITTER_OFF = -1;
242 int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &TRANSMITTER_OFF,
sizeof(TRANSMITTER_OFF));
243 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
245 HandleError(errorCode);
258 SwitchTransmitter(
false);
260 int errorCode = CloseBIRDSystem();
263 if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
264 HandleError(errorCode);
267 if (m_TransmitterConfig != NULL)
269 delete [] m_TransmitterConfig;
270 m_TransmitterConfig = NULL;
272 if (m_SensorConfig != NULL)
274 delete [] m_SensorConfig;
275 m_SensorConfig = NULL;
278 this->SetState(
Setup);
281 this->SetErrorMessage(
"");
290 struct itk::MultiThreader::ThreadInfoStruct * pInfo = (
struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
291 if ((pInfo == NULL) || (pInfo->UserData == NULL))
292 return ITK_THREAD_RETURN_VALUE;
295 if (trackingDevice != NULL)
298 return ITK_THREAD_RETURN_VALUE;
306 SwitchTransmitter(
false);
314 if (this->GetState() != Ready)
317 this->SetState(Tracking);
320 SwitchTransmitter(
true);
323 this->m_StopTrackingMutex->Lock();
324 this->m_StopTracking =
false;
325 this->m_StopTrackingMutex->Unlock();
327 m_TrackingFinishedMutex->Unlock();
328 m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking,
this);
329 mitk::TimeStamp::GetInstance()->Start(
this);
336 if (this->GetState() != Tracking)
340 double updateRate = 1000.0 / m_SystemConfig.measurementRate;
341 double measurementDuration = 0.0;
350 bool localStopTracking;
353 this->m_StopTrackingMutex->Lock();
354 localStopTracking = this->m_StopTracking;
355 this->m_StopTrackingMutex->Unlock();
358 while ((this->GetState() == Tracking) && (localStopTracking ==
false))
361 unsigned int nOfAttachedSensors = 0;
362 double timeStamp = 0.0;
365 for (
int sensorID = 0; sensorID < m_SystemConfig.numberSensors; sensorID++)
367 if (!m_SensorConfig[sensorID].attached)
371 errorCode = GetAsynchronousRecord(sensorID, pRecord,
sizeof(record));
372 if (CompareError(errorCode, BIRD_ERROR_SUCCESS))
374 nOfAttachedSensors++;
375 timeStamp += record.time;
376 ToolType* tool = GetMicroBirdTool(toolNumber);
381 position[0] = record.x;
382 position[1] = record.y;
383 position[2] = record.z;
385 mitk::Quaternion orientation(record.q[1], record.q[2], record.q[3],record.q[0]);
393 HandleError(errorCode);
401 double sleepTime = updateRate - measurementDuration;
403 if (sleepTime > 0.0 && sleepTime < 500.0)
407 itksys::SystemTools::Delay(sleepTime)
412 this->m_StopTrackingMutex->Lock();
413 localStopTracking = m_StopTracking;
414 this->m_StopTrackingMutex->Unlock();
427 return static_cast<TrackingTool*
>(GetMicroBirdTool(toolNumber));
436 if (toolNumber < m_Tools.size())
438 t = m_Tools.at(toolNumber);
447 return m_Tools.size();
453 return ((errorCode & 0xffff) == errorConstant);
460 char* pBuffer = &buffer[0];
462 while(!CompareError(errorCode, BIRD_ERROR_SUCCESS))
467 errorCode = GetErrorText(errorCode, pBuffer,
sizeof(buffer), SIMPLE_MESSAGE);
469 this->SetErrorMessage(buffer);
477 for (ToolContainerType::iterator iterator = m_Tools.begin(); iterator != m_Tools.end(); ++iterator)
478 (*iterator)->SetDataValid(
false);
virtual bool CloseConnection()
Closes the connection.
superclass for specific MIRCOBIRD tracking Devices
virtual TrackingTool * GetTool(unsigned int toolNumber)
returns a tracking tool that contains positional information about one of the sensors ...
virtual void InvalidateAll()
invalidates all tools (on stoptracking, closeconnection)
void HandleError(int errorCode)
bool CompareError(int errorCode, int errorConstant)
itk::FastMutexLock::Pointer m_ToolsMutex
virtual bool OpenConnection()
Builds up the connection (loads tools, initializes and enables them)
virtual unsigned int GetToolCount() const
returns a the number of attached sensors
itk::MultiThreader::Pointer m_MultiThreader
itk::MutexLockHolder< itk::FastMutexLock > MutexLockHolder
virtual bool StartTracking()
Start the tracking.
static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void *data)
Helper function, because the itk::MultiThreader can only start a new thread with a static member func...
virtual bool StopTracking()
stop retrieving tracking data from the device. stop retrieving tracking data from the device...
MicroBirdTrackingDevice()
DOUBLE_POSITION_QUATERNION_TIME_Q_RECORD * pRecord
One tracking data record (quaternion orientation format)
bool SwitchTransmitter(bool switchOn)
Switches the transmitter on resp. off.
Interface for all Tracking Devices.
virtual ~MicroBirdTrackingDevice()
vnl_quaternion< ScalarType > Quaternion
ToolContainerType m_Tools
virtual bool StopTracking()
here we use the superclass method.
TrackingDeviceData m_Data
current device Data
virtual void TrackTools()
tracks the position and orientation of all tools until StopTracking() is called.
DOUBLE_POSITION_QUATERNION_TIME_Q_RECORD record
ToolType * GetMicroBirdTool(unsigned int toolNumber)
returns a tracking tool that contains positional information about one of the sensors ...
static itkEventMacro(BoundingShapeInteractionEvent, itk::AnyEvent) class MITKBOUNDINGSHAPE_EXPORT BoundingShapeInteractor Pointer New()
Basic interaction methods for mitk::GeometryData.