vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
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>
6static const q_type gs_qIdent = Q_ID_QUAT;
7
8// We've not yet initialized the FreeSpace library.
9bool 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)
13static 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.
23static 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
29vrpn_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
44void 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
55void vrpn_Freespace::deviceSetConfiguration(bool send_body_frames, bool send_user_frames) {
56 _sendBodyFrames = send_body_frames;
57 _sendUserFrames = send_user_frames;
58}
59
60void 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
92void 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
124vrpn_Freespace* vrpn_Freespace::create(const char *name, vrpn_Connection *conn,
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;
159 try { dev = new vrpn_Freespace(freespaceId, &deviceInfo, name, conn); }
160 catch (...) { return NULL; }
161 dev->deviceSetConfiguration(send_body_frames, send_user_frames);
162 printf("Added freespace device: %s : \n", name);
163 return dev;
164}
165
167{
169 struct freespace_message s;
170
171 /*
172 * Send configuration information to the device repeatedly to force the
173 * device into the correct mode. This method ensures the correct mode
174 * even if the Freespace device was asleep during initialization or if
175 * the tracker resets.
176 */
177 struct timeval timestamp;
179 if (_timestamp.tv_sec != timestamp.tv_sec) {
180 _timestamp.tv_sec = timestamp.tv_sec;
181 deviceConfigure();
182 }
183
184 // Do not block, read as many messages as
185 while (FREESPACE_SUCCESS == freespace_readMessage(_freespaceDevice, &s, 0)) {
186 switch(s.messageType) {
187 case FREESPACE_MESSAGE_LINKSTATUS:
188 handleLinkStatus(s.linkStatus);
189 break;
190 case FREESPACE_MESSAGE_BODYFRAME:
191 handleBodyFrame(s.bodyFrame);
192 break;
193 case FREESPACE_MESSAGE_USERFRAME:
194 handleUserFrame(s.userFrame);
195 break;
196 case FREESPACE_MESSAGE_ALWAYSONRESPONSE:
197 break;
198 case FREESPACE_MESSAGE_DATAMODERESPONSE:
199 break;
200 default:
201 send_text_message("Received an unhandled message from freespace device", timestamp, vrpn_TEXT_WARNING);
202 break;
203 }
204 }
205}
207{
208 // as part of cleanup, disable user and body frame messages, and
209 // put back to just mouse movement.
210 int rc = freespace_flush(_freespaceDevice);
211 if (rc != FREESPACE_SUCCESS) {
212 printf("freespaceInputThread: Error flushing device: %d\n", rc);
213 }
214
215 deviceUnconfigure();
216 freespace_closeDevice(_freespaceDevice);
217}
218
219void vrpn_Freespace::handleBodyFrame(const struct freespace_BodyFrame& body) {
221 vrpn_float64 now = vrpn_Tracker::timestamp.tv_sec + MILLIS_TO_SECONDS(vrpn_Tracker::timestamp.tv_usec);
222 vrpn_float64 delta = now - _lastBodyFrameTime;
223 _lastBodyFrameTime = now;
224
225 vrpn_Tracker::acc[0] = convertFreespaceLinearAccelation(body.linearAccelX);
226 vrpn_Tracker::acc[1] = convertFreespaceLinearAccelation(body.linearAccelY);
227 vrpn_Tracker::acc[2] = convertFreespaceLinearAccelation(body.linearAccelZ);
228 q_copy(vrpn_Tracker::acc_quat, gs_qIdent);
232
233 vrpn_Tracker::vel[0] = convertFreespaceAngularVelocity(body.angularVelX, 1);
234 vrpn_Tracker::vel[1] = convertFreespaceAngularVelocity(body.angularVelY, 1);
235 vrpn_Tracker::vel[2] = convertFreespaceAngularVelocity(body.angularVelZ, 1);
236 q_copy(vrpn_Tracker::vel_quat, gs_qIdent);
240
241 vrpn_Button::buttons[0] = body.button1;
242 vrpn_Button::buttons[1] = body.button2;
243 vrpn_Button::buttons[2] = body.button3;
244 vrpn_Button::buttons[3] = body.button4;
245 vrpn_Button::buttons[4] = body.button5;
246
249
250 // this is totally arbitrary
251 // I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems
252 // like it would be the number of 'clicks' while turning the scroll wheel...
253 // vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means
254 // the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full
255 // revolution....'
256 // this values should probably set in a configuration file.
257 vrpn_Dial::dials[0] = body.deltaWheel / (vrpn_float64) 16;
260}
261
262void vrpn_Freespace::handleUserFrame(const struct freespace_UserFrame& user) {
264
265 // Translate the Freespace linear position into VRPN linear position.
266 vrpn_Tracker::pos[0] = user.linearPosY;
267 vrpn_Tracker::pos[1] = user.linearPosZ;
268 vrpn_Tracker::pos[2] = user.linearPosX;
269
270 // Transfer the Freespace coordinate system angular position quanternion
271 // into the VRPN coordinate system.
272 vrpn_Tracker::d_quat[Q_W] = user.angularPosA;
273 vrpn_Tracker::d_quat[Q_X] = user.angularPosC;
274 vrpn_Tracker::d_quat[Q_Y] = -user.angularPosD;
275 vrpn_Tracker::d_quat[Q_Z] = user.angularPosB;
277
279
280 vrpn_Button::buttons[0] = user.button1;
281 vrpn_Button::buttons[1] = user.button2;
282 vrpn_Button::buttons[2] = user.button3;
283 vrpn_Button::buttons[3] = user.button4;
284 vrpn_Button::buttons[4] = user.button5;
285
288
289 // this is totally arbitrary
290 // I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems
291 // like it would be the number of 'clicks' while turning the scroll wheel...
292 // vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means
293 // the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full
294 // revolution....'
295 // this values should probably set in a configuration file.
296 vrpn_Dial::dials[0] = user.deltaWheel / (vrpn_float64) 16;
299}
300void vrpn_Freespace::handleLinkStatus(const struct freespace_LinkStatus& l) {
301 // Could be used to send messages when the loop is powered off/unavailable.
302}
303#endif //VRPN_USE_FREESPACE
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
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.
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition vrpn_Button.h:66
vrpn_int32 num_buttons
Definition vrpn_Button.h:48
struct timeval timestamp
Definition vrpn_Button.h:49
virtual void report_changes(void)
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
virtual void report_changes(void)
Definition vrpn_Dial.C:54
struct timeval timestamp
Definition vrpn_Dial.h:28
vrpn_float64 dials[vrpn_DIAL_MAX]
Definition vrpn_Dial.h:26
vrpn_int32 num_dials
Definition vrpn_Dial.h:27
virtual void mainloop(void)
Main loop.
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.
FreespaceDeviceId _freespaceDevice
virtual ~vrpn_Freespace(void)
FreespaceDeviceInfo _deviceInfo
vrpn_float64 _lastBodyFrameTime
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)
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.
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)
vrpn_float64 acc_quat[4]
vrpn_float64 vel_quat[4]
vrpn_float64 d_quat[4]
vrpn_float64 vel_quat_dt
vrpn_float64 vel[3]
vrpn_float64 acc_quat_dt
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_float64 acc[3]
@ vrpn_TEXT_WARNING
#define MILLIS_TO_SECONDS(x)
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99