vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
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
17const unsigned VENDOR_ID = 0x09d9;
18const unsigned PRODUCT_ID = 0x64df;
19
20using namespace std;
21
22//WintrackerIII Data length + 1 Byte for HID Report ID (0x0 as the device has no report IDs)
23
24#ifdef VRPN_USE_HID
25vrpn_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):
27{
28 _name = name;
29 _con = trackercon;
30
31 memset(d_quat, 0, 4 * sizeof(float));
32 memset(pos, 0, 3 * sizeof(float));
33
35
36 //configure WintrackerIII sensors activation state
37 const vrpn_uint8 sensor0 = s0;
38 const vrpn_uint8 sensor1 = s1;
39 const vrpn_uint8 sensor2 = s2;
40 const vrpn_uint8 set_sensors_cmd[] = {0x0,'S','A',sensor0,sensor1,sensor2,'\n'};
41 //Send sensors activation data to the WintrackerIII device
42 send_data(sizeof(set_sensors_cmd),set_sensors_cmd);
43
44 cout << "WintrackerIII Vrpn Server up and running..." << endl;
45 cout << "Sensors activation state : " << endl;
46 cout << "Sensor 0: " << sensor0 << " - " << "Sensor 1: " << sensor1 << " - " << "Sensor 2: " << sensor2 <<endl;
47
48 //if parameter hemisphere is set to Z, change the hemisphere of operation to the upper one (Z<0)
49 //by default set to front one (X>0)
50 if(hemisphere=='Z'){
51 cout << "Hemisphere of operation : Upper(Z<0)" << endl;
52 const vrpn_uint8 set_hemi[] = {0x0,'S','H','U','\n'};
53 send_data(sizeof(set_hemi),set_hemi);
54 }
55 else{
56 cout << "Hemisphere of operation : Front(X>0)" << endl;
57 }
58
59 //if parameter ext == 1, activate VRSpace Range Extender
60 if(ext=='1'){
61 cout << "Range Extender : Activated" << endl;
62 const vrpn_uint8 set_extender_cmd[] = {0x0,'S','L','\n'};
63 send_data(sizeof(set_extender_cmd),set_extender_cmd);
64 }
65 else{
66 cout << "Range Extender : Not Activated" << endl;
67 }
68
70}
71
72
73void vrpn_Tracker_Wintracker::on_data_received(size_t bytes, vrpn_uint8 *buff)
74{
75
76 if (bytes == 24) {
77
78 vrpn_uint8 recordType = vrpn_unbuffer_from_little_endian<vrpn_int8>(buff);
79 //second byte of the buffer, identifying the sensor number
80 vrpn_uint8 recordNumber = vrpn_unbuffer_from_little_endian<vrpn_int8>(buff);
81
82 //WintrackerIII identifies the sensors using recordNumber
83 //48=sensor 0
84 //49=sensor 1
85 //50=sensor 2
86 if(((int)recordNumber)==48){
87 d_sensor = 0;
88 }else if(((int)recordNumber)==49){
89 d_sensor = 1;
90 }else if(((int)recordNumber)==50){
91 d_sensor = 2;
92 }
93
94 //position data (transformed in meters from WintrackerIII 0.1 mm data
95 pos[0]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
96 pos[1]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
97 pos[2]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
98
99 //azimuth, elevation and roll data, not used by the vrpn server
100 vrpn_uint16 azimuth = vrpn_unbuffer_from_little_endian<vrpn_int16>(buff);
101 vrpn_uint16 elevation = vrpn_unbuffer_from_little_endian<vrpn_int16>(buff);
102 vrpn_uint16 roll = vrpn_unbuffer_from_little_endian<vrpn_int16>(buff);
103
104 //normalized quaternion data (Vrpn Order Qx,Qy,Qz,Qw - WintrackerIII Order Qw,Qx,Qy,Qz)
105 d_quat[3]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
106 d_quat[0]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
107 d_quat[1]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
108 d_quat[2]=vrpn_unbuffer_from_little_endian<vrpn_int16>(buff)/10000.00;
109
112
113 char msgbuf[1000];
114
115 int len = vrpn_Tracker::encode_to(msgbuf);
116
118 fprintf(stderr, "FAIL \n");
119 }
120
121 // Use unused variables to avoid compiler warnings.
122 recordType = recordType + 1;
123 azimuth = azimuth + 1;
124 elevation = elevation + 1;
125 roll = roll + 1;
126 } else {
127 fprintf(stderr, "FAIL : Cannot read input from Wintracker \n");
128 }
129}
130
131
133{
134 if (connected())
135 {
136 // device update
137 update();
138
139 // server update
141
142 }
143}
144
145#endif
146
147
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Generic connection class not specific to the transport mechanism.
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...
virtual void update()
Polls the device buffers and causes on_data_received callbacks if appropriate You NEED to call this f...
virtual bool connected() const
Returns true iff the last device I/O succeeded.
void send_data(size_t bytes, const vrpn_uint8 *buffer)
Call this to send data to the device.
Accepts any device with the given vendor and product IDs.
virtual void on_data_received(size_t bytes, vrpn_uint8 *buffer)
Derived class reimplements this callback.
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_Tracker_Wintracker(const char *name, vrpn_Connection *trackercon, const char s0, const char s1, const char s2, const char ext, const char hemisphere)
virtual int encode_to(char *buf)
vrpn_float64 d_quat[4]
vrpn_int32 d_sensor
vrpn_float64 pos[3]
vrpn_int32 num_sensors
struct timeval timestamp
vrpn_int32 position_m_id
#define VRPN_SUPPRESS_EMPTY_OBJECT_WARNING()
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
const unsigned VENDOR_ID
const unsigned PRODUCT_ID