vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Freespace.C
Go to the documentation of this file.
1 #include "vrpn_Freespace.h"
2 #ifdef VRPN_USE_FREESPACE
3 #include "quat.h"
4 #include <freespace/freespace_codecs.h>
5 #include <cstring>
6 static const q_type gs_qIdent = Q_ID_QUAT;
7 
8 // We've not yet initialized the FreeSpace library.
9 bool vrpn_Freespace::_freespace_initialized = false;
10 
11 // libfreespace provides linear acceleration values in (mm/(s^2))^-3
12 // VRPN wants these in meters/(s^2)
13 static vrpn_float64 convertFreespaceLinearAccelation(int16_t mmpss) {
14  return mmpss / ((vrpn_float64) 1000);
15 }
16 // freespace reports in milliradians per second, so need to convert
17 // to meters per second...
18 // The radius to use is a bit arbitrary, and would depend on
19 // what exactly this device is 'attached' to. Additionally, this may
20 // be different for each axis. In general, the radius shouldn't really
21 // matter since the device may be representing a larger/smaller object in
22 // the virtual world.
23 static vrpn_float64 convertFreespaceAngularVelocity(int16_t mrps, double radius) {
24  return mrps * radius / ((vrpn_float64) 1000);
25 }
26 #define MILLIS_TO_SECONDS(x) (x / ((vrpn_float64) 1000000))
27 
28 
29 vrpn_Freespace::vrpn_Freespace(FreespaceDeviceId freespaceId,
30  struct FreespaceDeviceInfo* deviceInfo,
31  const char *name, vrpn_Connection *conn):
32  vrpn_Tracker_Server(name, conn),
33  vrpn_Button_Filter(name, conn),
34  vrpn_Dial(name, conn)
35 {
36  memcpy(&_deviceInfo, deviceInfo, sizeof(_deviceInfo));
37  // 5 buttons + a scroll wheel
40  memset(&_lastBodyFrameTime, 0, sizeof(_lastBodyFrameTime));
41  _timestamp.tv_sec = 0;
42 }
43 
44 void vrpn_Freespace::freespaceInit() {
45  if (!_freespace_initialized) {
46  _freespace_initialized = true;
47  int rc = freespace_init();
48  if (rc != FREESPACE_SUCCESS) {
49  fprintf(stderr, "vrpn_Freespace::freespaceInit: failed to init freespace lib. rc=%d\n", rc);
50  _freespace_initialized = false;
51  }
52  }
53 }
54 
55 void vrpn_Freespace::deviceSetConfiguration(bool send_body_frames, bool send_user_frames) {
56  _sendBodyFrames = send_body_frames;
57  _sendUserFrames = send_user_frames;
58 }
59 
60 void vrpn_Freespace::deviceConfigure() {
61  int rc = 0;
62 
63  // Send the data mode request
64  struct freespace_message message;
65  memset(&message, 0, sizeof(message));
66  if (_deviceInfo.hVer == 1) {
67  message.messageType = FREESPACE_MESSAGE_DATAMOTIONCONTROL;
68  struct freespace_DataMotionControl * req;
69  req = &message.dataMotionControl;
70  if (_sendBodyFrames) { req->enableBodyMotion = 1; }
71  if (_sendUserFrames) { req->enableUserPosition = 1; }
72  req->inhibitPowerManager = 1;
73  req->enableMouseMovement = 0;
74  req->disableFreespace = 0;
75  } else {
76  message.messageType = FREESPACE_MESSAGE_DATAMODEREQUEST;
77  struct freespace_DataModeRequest * req;
78  req = &message.dataModeRequest;
79  if (_sendBodyFrames) { req->enableBodyMotion = 1; }
80  if (_sendUserFrames) { req->enableUserPosition = 1; }
81  req->inhibitPowerManager = 1;
82  req->enableMouseMovement = 0;
83  req->disableFreespace = 0;
84  }
85 
86  rc = freespace_sendMessage(_freespaceDevice, &message);
87  if (rc != FREESPACE_SUCCESS) {
88  fprintf(stderr, "vrpn_Freespace::deviceConfigure: Could not send message: %d.\n", rc);
89  }
90 }
91 
92 void vrpn_Freespace::deviceUnconfigure() {
93  int rc = 0;
94 
95  // Send the data mode request
96  struct freespace_message message;
97  memset(&message, 0, sizeof(message));
98  if (_deviceInfo.hVer == 1) {
99  message.messageType = FREESPACE_MESSAGE_DATAMOTIONCONTROL;
100  struct freespace_DataMotionControl * req;
101  req = &message.dataMotionControl;
102  req->enableBodyMotion = 0;
103  req->enableUserPosition = 0;
104  req->inhibitPowerManager = 0;
105  req->enableMouseMovement = 1;
106  req->disableFreespace = 0;
107  } else {
108  message.messageType = FREESPACE_MESSAGE_DATAMODEREQUEST;
109  struct freespace_DataModeRequest * req;
110  req = &message.dataModeRequest;
111  req->enableBodyMotion = 0;
112  req->enableUserPosition = 0;
113  req->inhibitPowerManager = 0;
114  req->enableMouseMovement = 1;
115  req->disableFreespace = 0;
116  }
117 
118  rc = freespace_sendMessage(_freespaceDevice, &message);
119  if (rc != FREESPACE_SUCCESS) {
120  fprintf(stderr, "vrpn_Freespace::deviceUnconfigure: Could not send message: %d.\n", rc);
121  }
122 }
123 
125  int device_index,
126  bool send_body_frames, bool send_user_frames)
127 {
128  // initialize libfreespace
129  vrpn_Freespace::freespaceInit();
130 
131  const unsigned MAX_DEVICES = 100;
132  FreespaceDeviceId devices[MAX_DEVICES];
133  int numIds = 0;
134  int rc;
135  rc = freespace_getDeviceList(devices, MAX_DEVICES, &numIds);
136  if ( (rc != FREESPACE_SUCCESS) || (numIds < (device_index+1)) ) {
137  fprintf(stderr, "vrpn_Freespace::create: Didn't find enough devices: %d.\n", numIds);
138  return NULL;
139  }
140  FreespaceDeviceId freespaceId = devices[device_index];
141 
142  rc = freespace_openDevice(freespaceId);
143  if (rc != FREESPACE_SUCCESS) {
144  fprintf(stderr, "vrpn_Freespace::create: Could not open device %d.\n", device_index);
145  return NULL;
146  }
147  struct FreespaceDeviceInfo deviceInfo;
148  rc = freespace_getDeviceInfo(freespaceId, &deviceInfo);
149  if (rc != FREESPACE_SUCCESS) {
150  return NULL;
151  }
152  rc = freespace_flush(freespaceId);
153  if (rc != FREESPACE_SUCCESS) {
154  fprintf(stderr, "vrpn_Freespace::create: Error flushing device: %d\n", rc);
155  return NULL;
156  }
157 
158  vrpn_Freespace* dev = new vrpn_Freespace(freespaceId, &deviceInfo, name, conn);
159  dev->deviceSetConfiguration(send_body_frames, send_user_frames);
160  printf("Added freespace device: %s : \n", name);
161  return dev;
162 }
163 
165 {
166  server_mainloop();
167  struct freespace_message s;
168 
169  /*
170  * Send configuration information to the device repeatedly to force the
171  * device into the correct mode. This method ensures the correct mode
172  * even if the Freespace device was asleep during initialization or if
173  * the tracker resets.
174  */
175  struct timeval timestamp;
177  if (_timestamp.tv_sec != timestamp.tv_sec) {
178  _timestamp.tv_sec = timestamp.tv_sec;
179  deviceConfigure();
180  }
181 
182  // Do not block, read as many messages as
183  while (FREESPACE_SUCCESS == freespace_readMessage(_freespaceDevice, &s, 0)) {
184  switch(s.messageType) {
185  case FREESPACE_MESSAGE_LINKSTATUS:
186  handleLinkStatus(s.linkStatus);
187  break;
188  case FREESPACE_MESSAGE_BODYFRAME:
189  handleBodyFrame(s.bodyFrame);
190  break;
191  case FREESPACE_MESSAGE_USERFRAME:
192  handleUserFrame(s.userFrame);
193  break;
194  case FREESPACE_MESSAGE_ALWAYSONRESPONSE:
195  break;
196  case FREESPACE_MESSAGE_DATAMODERESPONSE:
197  break;
198  default:
199  send_text_message("Received an unhandled message from freespace device", timestamp, vrpn_TEXT_WARNING);
200  break;
201  }
202  }
203 }
205 {
206  // as part of cleanup, disable user and body frame messages, and
207  // put back to just mouse movement.
208  int rc = freespace_flush(_freespaceDevice);
209  if (rc != FREESPACE_SUCCESS) {
210  printf("freespaceInputThread: Error flushing device: %d\n", rc);
211  }
212 
213  deviceUnconfigure();
214  freespace_closeDevice(_freespaceDevice);
215 }
216 
217 void vrpn_Freespace::handleBodyFrame(const struct freespace_BodyFrame& body) {
219  vrpn_float64 now = vrpn_Tracker::timestamp.tv_sec + MILLIS_TO_SECONDS(vrpn_Tracker::timestamp.tv_usec);
220  vrpn_float64 delta = now - _lastBodyFrameTime;
221  _lastBodyFrameTime = now;
222 
223  vrpn_Tracker::acc[0] = convertFreespaceLinearAccelation(body.linearAccelX);
224  vrpn_Tracker::acc[1] = convertFreespaceLinearAccelation(body.linearAccelY);
225  vrpn_Tracker::acc[2] = convertFreespaceLinearAccelation(body.linearAccelZ);
226  q_copy(vrpn_Tracker::acc_quat, gs_qIdent);
230 
231  vrpn_Tracker::vel[0] = convertFreespaceAngularVelocity(body.angularVelX, 1);
232  vrpn_Tracker::vel[1] = convertFreespaceAngularVelocity(body.angularVelY, 1);
233  vrpn_Tracker::vel[2] = convertFreespaceAngularVelocity(body.angularVelZ, 1);
234  q_copy(vrpn_Tracker::vel_quat, gs_qIdent);
238 
239  vrpn_Button::buttons[0] = body.button1;
240  vrpn_Button::buttons[1] = body.button2;
241  vrpn_Button::buttons[2] = body.button3;
242  vrpn_Button::buttons[3] = body.button4;
243  vrpn_Button::buttons[4] = body.button5;
244 
247 
248  // this is totally arbitrary
249  // I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems
250  // like it would be the number of 'clicks' while turning the scroll wheel...
251  // vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means
252  // the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full
253  // revolution....'
254  // this values should probably set in a configuration file.
255  vrpn_Dial::dials[0] = body.deltaWheel / (vrpn_float64) 16;
258 }
259 
260 void vrpn_Freespace::handleUserFrame(const struct freespace_UserFrame& user) {
262 
263  // Translate the Freespace linear position into VRPN linear position.
264  vrpn_Tracker::pos[0] = user.linearPosY;
265  vrpn_Tracker::pos[1] = user.linearPosZ;
266  vrpn_Tracker::pos[2] = user.linearPosX;
267 
268  // Transfer the Freespace coordinate system angular position quanternion
269  // into the VRPN coordinate system.
270  vrpn_Tracker::d_quat[Q_W] = user.angularPosA;
271  vrpn_Tracker::d_quat[Q_X] = user.angularPosC;
272  vrpn_Tracker::d_quat[Q_Y] = -user.angularPosD;
273  vrpn_Tracker::d_quat[Q_Z] = user.angularPosB;
275 
277 
278  vrpn_Button::buttons[0] = user.button1;
279  vrpn_Button::buttons[1] = user.button2;
280  vrpn_Button::buttons[2] = user.button3;
281  vrpn_Button::buttons[3] = user.button4;
282  vrpn_Button::buttons[4] = user.button5;
283 
286 
287  // this is totally arbitrary
288  // I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems
289  // like it would be the number of 'clicks' while turning the scroll wheel...
290  // vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means
291  // the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full
292  // revolution....'
293  // this values should probably set in a configuration file.
294  vrpn_Dial::dials[0] = user.deltaWheel / (vrpn_float64) 16;
297 }
298 void vrpn_Freespace::handleLinkStatus(const struct freespace_LinkStatus& l) {
299  // Could be used to send messages when the loop is powered off/unavailable.
300 }
301 #endif //VRPN_USE_FREESPACE
vrpn_Tracker::acc
vrpn_float64 acc[3]
Definition: vrpn_Tracker.h:98
vrpn_Freespace::~vrpn_Freespace
virtual ~vrpn_Freespace(void)
Definition: vrpn_Freespace.C:204
vrpn_Tracker::vel
vrpn_float64 vel[3]
Definition: vrpn_Tracker.h:96
vrpn_Tracker_Server::report_pose_acceleration
virtual int report_pose_acceleration(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Definition: vrpn_Tracker.C:807
vrpn_Button::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_Tracker_Server::report_pose_velocity
virtual int report_pose_velocity(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Definition: vrpn_Tracker.C:767
vrpn_Freespace::_lastBodyFrameTime
vrpn_float64 _lastBodyFrameTime
Definition: vrpn_Freespace.h:94
vrpn_Tracker_Server
Definition: vrpn_Tracker.h:251
vrpn_Tracker_Server::report_pose
virtual int report_pose(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
These functions should be called to report changes in state, once per sensor.
Definition: vrpn_Tracker.C:729
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_Button::num_buttons
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_Button::buttons
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
vrpn_Tracker::acc_quat
vrpn_float64 acc_quat[4]
Definition: vrpn_Tracker.h:98
vrpn_Dial
Definition: vrpn_Dial.h:21
vrpn_TEXT_WARNING
@ vrpn_TEXT_WARNING
Definition: vrpn_BaseClass.h:102
vrpn_Button::timestamp
struct timeval timestamp
Definition: vrpn_Button.h:48
vrpn_Dial::dials
vrpn_float64 dials[vrpn_DIAL_MAX]
Definition: vrpn_Dial.h:26
vrpn_Tracker::acc_quat_dt
vrpn_float64 acc_quat_dt
Definition: vrpn_Tracker.h:99
vrpn_Dial::report_changes
virtual void report_changes(void)
Definition: vrpn_Dial.C:54
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
MILLIS_TO_SECONDS
#define MILLIS_TO_SECONDS(x)
Definition: vrpn_Freespace.C:26
vrpn_Freespace::create
static vrpn_Freespace * create(const char *name, vrpn_Connection *conn, int device_index=0, bool send_body_frames=false, bool send_user_frames=true)
Create a freespace server using the given FreespaceDeviceId.
Definition: vrpn_Freespace.C:124
vrpn_Tracker::vel_quat
vrpn_float64 vel_quat[4]
Definition: vrpn_Tracker.h:96
vrpn_BaseClassUnique::send_text_message
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
Definition: vrpn_BaseClass.C:568
vrpn_Freespace::_freespaceDevice
FreespaceDeviceId _freespaceDevice
Definition: vrpn_Freespace.h:92
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_Dial::num_dials
vrpn_int32 num_dials
Definition: vrpn_Dial.h:27
vrpn_Dial::timestamp
struct timeval timestamp
Definition: vrpn_Dial.h:28
vrpn_Tracker::vel_quat_dt
vrpn_float64 vel_quat_dt
Definition: vrpn_Tracker.h:97
vrpn_Freespace::mainloop
virtual void mainloop(void)
Main loop.
Definition: vrpn_Freespace.C:164
vrpn_Freespace::_deviceInfo
FreespaceDeviceInfo _deviceInfo
Definition: vrpn_Freespace.h:93
vrpn_Freespace.h
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
vrpn_Freespace
This provides an interface to devices powered by Hillcrest Lab's Freespace.
Definition: vrpn_Freespace.h:33
vrpn_Button_Filter
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65