vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_Wintracker.C
Go to the documentation of this file.
1 /*
2  * vrpn_Tracker_Wintracker.cpp
3  *
4  * Created on: Dec 7, 2012
5  * Author: Emiliano Pastorelli - Institute of Cybernetics, Tallinn (Estonia)
6  */
7 #include <stdio.h>
8 #include <string.h>
9 #include <stdlib.h>
10 #include <iostream>
11 
12 #include "quat.h"
14 
16 
17 //using namespace std;
18 
19 const unsigned VENDOR_ID = 0x09d9;
20 const unsigned PRODUCT_ID = 0x64df;
21 
22 using namespace std;
23 
24 //WintrackerIII Data length + 1 Byte for HID Report ID (0x0 as the device has no report IDs)
25 
26 
27 
28 #ifdef VRPN_USE_HID
29 vrpn_Tracker_Wintracker::vrpn_Tracker_Wintracker(const char * name, vrpn_Connection * trackercon, const char s0, const char s1, const char s2, const char ext, const char hemisphere):
31  _name = name;
32  _con = trackercon;
33 
34  memset(d_quat, 0, 4 * sizeof(float));
35  memset(pos, 0, 3 * sizeof(float));
36 
38 
39  //configure WintrackerIII sensors activation state
40  const vrpn_uint8 sensor0 = s0;
41  const vrpn_uint8 sensor1 = s1;
42  const vrpn_uint8 sensor2 = s2;
43  const vrpn_uint8 set_sensors_cmd[] = {0x0,'S','A',sensor0,sensor1,sensor2,'\n'};
44  //Send sensors activation data to the WintrackerIII device
45  send_data(sizeof(set_sensors_cmd),set_sensors_cmd);
46 
47  cout << "WintrackerIII Vrpn Server up and running..." << endl;
48  cout << "Sensors activation state : " << endl;
49  cout << "Sensor 0: " << sensor0 << " - " << "Sensor 1: " << sensor1 << " - " << "Sensor 2: " << sensor2 <<endl;
50 
51  //if parameter hemisphere is set to Z, change the hemisphere of operation to the upper one (Z<0)
52  //by default set to front one (X>0)
53  if(hemisphere=='Z'){
54  cout << "Hemisphere of operation : Upper(Z<0)" << endl;
55  const vrpn_uint8 set_hemi[] = {0x0,'S','H','U','\n'};
56  send_data(sizeof(set_hemi),set_hemi);
57  }
58  else{
59  cout << "Hemisphere of operation : Front(X>0)" << endl;
60  }
61 
62  //if parameter ext == 1, activate VRSpace Range Extender
63  if(ext=='1'){
64  cout << "Range Extender : Activated" << endl;
65  const vrpn_uint8 set_extender_cmd[] = {0x0,'S','L','\n'};
66  send_data(sizeof(set_extender_cmd),set_extender_cmd);
67  }
68  else{
69  cout << "Range Extender : Not Activated" << endl;
70  }
71 
73 }
74 
75 
76 void vrpn_Tracker_Wintracker::on_data_received(size_t bytes, vrpn_uint8 *buff){
77 
78  if (bytes == 24){
79 
80  vrpn_uint8 recordType = vrpn_unbuffer_from_little_endian<vrpn_int8>(buff);
81  //second byte of the buffer, identifying the sensor number
82  vrpn_uint8 recordNumber = vrpn_unbuffer_from_little_endian<vrpn_int8>(buff);
83 
84  //WintrackerIII identifies the sensors using recordNumber
85  //48=sensor 0
86  //49=sensor 1
87  //50=sensor 2
88  if(((int)recordNumber)==48){
89  d_sensor = 0;
90  }else if(((int)recordNumber)==49){
91  d_sensor = 1;
92  }else if(((int)recordNumber)==50){
93  d_sensor = 2;
94  }
95 
96  //position data (transformed in meters from WintrackerIII 0.1 mm data
97  pos[0]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
98  pos[1]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
99  pos[2]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
100 
101  //azimuth, elevation and roll data, not used by the vrpn server
102  vrpn_uint16 azimuth = vrpn_unbuffer_from_little_endian<vrpn_int16>(buff);
103  vrpn_uint16 elevation = vrpn_unbuffer_from_little_endian<vrpn_int16>(buff);
104  vrpn_uint16 roll = vrpn_unbuffer_from_little_endian<vrpn_int16>(buff);
105 
106  //normalized quaternion data (Vrpn Order Qx,Qy,Qz,Qw - WintrackerIII Order Qw,Qx,Qy,Qz)
107  d_quat[3]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
108  d_quat[0]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
109  d_quat[1]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
110  d_quat[2]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
111 
114 
115  char msgbuf[1000];
116 
117  int len = vrpn_Tracker::encode_to(msgbuf);
118 
120  fprintf(stderr, "FAIL \n");
121  }
122  }
123  else
124  {
125  fprintf(stderr, "FAIL : Cannot read input from Wintracker \n");
126  }
127 }
128 
129 
131 {
132  if (connected())
133  {
134  // device update
135  update();
136 
137  // server update
138  server_mainloop();
139 
140  }
141 }
142 
143 #endif
144 
145 
vrpn_Tracker_Wintracker::_timestamp
struct timeval _timestamp
Definition: vrpn_Tracker_Wintracker.h:33
vrpn_HidInterface::connected
virtual bool connected() const
Returns true iff the last device I/O succeeded.
Definition: vrpn_HumanInterface.C:23
vrpn_Tracker_Wintracker::vrpn_Tracker_Wintracker
vrpn_Tracker_Wintracker(const char *name, vrpn_Connection *trackercon, const char s0, const char s1, const char s2, const char ext, const char hemisphere)
Definition: vrpn_Tracker_Wintracker.C:29
vrpn_Connection::pack_message
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
Definition: vrpn_Connection.C:4632
vrpn_Tracker
Definition: vrpn_Tracker.h:49
vrpn_Tracker::encode_to
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_HidInterface
Definition: vrpn_HumanInterface.h:68
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_HidInterface::update
virtual void update()
Polls the device buffers and causes on_data_received callbacks if appropriate You NEED to call this f...
Definition: vrpn_HumanInterface.C:140
vrpn_Tracker_Wintracker::_name
std::string _name
Definition: vrpn_Tracker_Wintracker.h:29
vrpn_CONNECTION_LOW_LATENCY
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Definition: vrpn_Connection.h:122
vrpn_BaseClassUnique::d_connection
vrpn_Connection * d_connection
Connection that this object talks to.
Definition: vrpn_BaseClass.h:224
vrpn_Tracker_Wintracker::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Tracker_Wintracker.C:130
vrpn_Tracker_Wintracker::_con
vrpn_Connection * _con
Definition: vrpn_Tracker_Wintracker.h:30
vrpn_HidProductAcceptor
Accepts any device with the given vendor and product IDs.
Definition: vrpn_HumanInterface.h:150
vrpn_BaseClassUnique::d_sender_id
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Definition: vrpn_BaseClass.h:228
vrpn_Tracker::position_m_id
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
VENDOR_ID
const unsigned VENDOR_ID
Definition: vrpn_Tracker_Wintracker.C:19
PRODUCT_ID
const unsigned PRODUCT_ID
Definition: vrpn_Tracker_Wintracker.C:20
vrpn_Tracker_Wintracker.h
vrpn_Tracker_Wintracker::on_data_received
virtual void on_data_received(size_t bytes, vrpn_uint8 *buffer)
Derived class reimplements this callback.
Definition: vrpn_Tracker_Wintracker.C:76
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_Tracker::num_sensors
vrpn_int32 num_sensors
Definition: vrpn_Tracker.h:114
VRPN_SUPPRESS_EMPTY_OBJECT_WARNING
#define VRPN_SUPPRESS_EMPTY_OBJECT_WARNING()
Definition: vrpn_Configure.h:495
vrpn_HidInterface::send_data
void send_data(size_t bytes, const vrpn_uint8 *buffer)
Call this to send data to the device.
Definition: vrpn_HumanInterface.C:187
vrpn_BaseClassUnique::server_mainloop
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Definition: vrpn_BaseClass.C:603