2 #ifdef VRPN_USE_FREESPACE 4 #include <freespace/freespace_codecs.h> 6 static const q_type gs_qIdent = Q_ID_QUAT;
9 bool vrpn_Freespace::_freespace_initialized =
false;
13 static vrpn_float64 convertFreespaceLinearAccelation(int16_t mmpss) {
14 return mmpss / ((vrpn_float64) 1000);
23 static vrpn_float64 convertFreespaceAngularVelocity(int16_t mrps,
double radius) {
24 return mrps * radius / ((vrpn_float64) 1000);
26 #define MILLIS_TO_SECONDS(x) (x / ((vrpn_float64) 1000000)) 29 vrpn_Freespace::vrpn_Freespace(FreespaceDeviceId freespaceId,
30 struct FreespaceDeviceInfo* deviceInfo,
36 memcpy(&_deviceInfo, deviceInfo,
sizeof(_deviceInfo));
40 memset(&_lastBodyFrameTime, 0,
sizeof(_lastBodyFrameTime));
41 _timestamp.tv_sec = 0;
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;
55 void vrpn_Freespace::deviceSetConfiguration(
bool send_body_frames,
bool send_user_frames) {
56 _sendBodyFrames = send_body_frames;
57 _sendUserFrames = send_user_frames;
60 void vrpn_Freespace::deviceConfigure() {
64 struct freespace_message message;
65 memset(&message, 0,
sizeof(message));
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;
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;
87 if (rc != FREESPACE_SUCCESS) {
88 fprintf(stderr,
"vrpn_Freespace::deviceConfigure: Could not send message: %d.\n", rc);
92 void vrpn_Freespace::deviceUnconfigure() {
96 struct freespace_message message;
97 memset(&message, 0,
sizeof(message));
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;
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;
119 if (rc != FREESPACE_SUCCESS) {
120 fprintf(stderr,
"vrpn_Freespace::deviceUnconfigure: Could not send message: %d.\n", rc);
126 bool send_body_frames,
bool send_user_frames)
129 vrpn_Freespace::freespaceInit();
131 const unsigned MAX_DEVICES = 100;
132 FreespaceDeviceId devices[MAX_DEVICES];
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);
140 FreespaceDeviceId freespaceId = devices[device_index];
142 rc = freespace_openDevice(freespaceId);
143 if (rc != FREESPACE_SUCCESS) {
144 fprintf(stderr,
"vrpn_Freespace::create: Could not open device %d.\n", device_index);
147 struct FreespaceDeviceInfo deviceInfo;
148 rc = freespace_getDeviceInfo(freespaceId, &deviceInfo);
149 if (rc != FREESPACE_SUCCESS) {
152 rc = freespace_flush(freespaceId);
153 if (rc != FREESPACE_SUCCESS) {
154 fprintf(stderr,
"vrpn_Freespace::create: Error flushing device: %d\n", rc);
159 dev->deviceSetConfiguration(send_body_frames, send_user_frames);
160 printf(
"Added freespace device: %s : \n", name);
167 struct freespace_message s;
177 if (_timestamp.tv_sec !=
timestamp.tv_sec) {
183 while (FREESPACE_SUCCESS == freespace_readMessage(
_freespaceDevice, &s, 0)) {
184 switch(s.messageType) {
185 case FREESPACE_MESSAGE_LINKSTATUS:
186 handleLinkStatus(s.linkStatus);
188 case FREESPACE_MESSAGE_BODYFRAME:
189 handleBodyFrame(s.bodyFrame);
191 case FREESPACE_MESSAGE_USERFRAME:
192 handleUserFrame(s.userFrame);
194 case FREESPACE_MESSAGE_ALWAYSONRESPONSE:
196 case FREESPACE_MESSAGE_DATAMODERESPONSE:
209 if (rc != FREESPACE_SUCCESS) {
210 printf(
"freespaceInputThread: Error flushing device: %d\n", rc);
217 void vrpn_Freespace::handleBodyFrame(
const struct freespace_BodyFrame& body) {
260 void vrpn_Freespace::handleUserFrame(
const struct freespace_UserFrame& user) {
298 void vrpn_Freespace::handleLinkStatus(
const struct freespace_LinkStatus& l) {
301 #endif //VRPN_USE_FREESPACE This provides an interface to devices powered by Hillcrest Lab's Freespace.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual void report_changes(void)
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 ~vrpn_Freespace(void)
FreespaceDeviceInfo _deviceInfo
virtual void mainloop(void)
Main loop.
FreespaceDeviceId _freespaceDevice
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)
Generic connection class not specific to the transport mechanism.
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.
#define MILLIS_TO_SECONDS(x)
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.
vrpn_float64 _lastBodyFrameTime
#define vrpn_gettimeofday
vrpn_float64 dials[vrpn_DIAL_MAX]