void vrpn_Freespace::mainloop(void)
{
server_mainloop();
struct freespace_message s;
/*
* Send configuration information to the device repeatedly to force the
* device into the correct mode. This method ensures the correct mode
* even if the Freespace device was asleep during initialization or if
* the tracker resets.
*/
struct timeval timestamp;
vrpn_gettimeofday(×tamp, NULL);
if (_timestamp.tv_sec != timestamp.tv_sec) {
_timestamp.tv_sec = timestamp.tv_sec;
deviceConfigure();
}
// Do not block, read as many messages as
while (FREESPACE_SUCCESS == freespace_readMessage(_freespaceDevice, &s, 0)) {
switch(s.messageType) {
case FREESPACE_MESSAGE_LINKSTATUS:
handleLinkStatus(s.linkStatus);
break;
case FREESPACE_MESSAGE_BODYFRAME:
handleBodyFrame(s.bodyFrame);
break;
case FREESPACE_MESSAGE_USERFRAME:
handleUserFrame(s.userFrame);
break;
case FREESPACE_MESSAGE_ALWAYSONRESPONSE:
break;
case FREESPACE_MESSAGE_DATAMODERESPONSE:
break;
default:
send_text_message("Received an unhandled message from freespace device", timestamp, vrpn_TEXT_WARNING);
break;
}
}
}
int FreeSpaceTracker::getOrientationFromDevice(float* yaw, float* pitch, float* roll)
{
//OutputDebugString("Free Tracker getOrient\n");
freespace_message msg;
int err = freespace_readMessage(DeviceID, &msg, 10);
/*
char errChar[512];
sprintf_s(errChar, "devID = %d, err == %d", DeviceID, err);
OutputDebugString(errChar);
OutputDebugString("\n");
*/
if (err == 0)
{
// Check if this is a user frame message.
if (msg.messageType == FREESPACE_MESSAGE_USERFRAME)
{
// Convert from quaternion to Euler angles
// Get the quaternion vector
float w = msg.userFrame.angularPosA;
float x = msg.userFrame.angularPosB;
float y = msg.userFrame.angularPosC;
float z = msg.userFrame.angularPosD;
// normalize the vector
float len = sqrtf((w*w) + (x*x) + (y*y) + (z*z));
w /= len;
x /= len;
y /= len;
z /= len;
// The Freespace quaternion gives the rotation in terms of
// rotating the world around the object. We take the conjugate to
// get the rotation in the object's reference frame.
w = w;
x = -x;
y = -y;
z = -z;
// Convert to angles in radians
float m11 = (2.0f * w * w) + (2.0f * x * x) - 1.0f;
float m12 = (2.0f * x * y) + (2.0f * w * z);
float m13 = (2.0f * x * z) - (2.0f * w * y);
float m23 = (2.0f * y * z) + (2.0f * w * x);
float m33 = (2.0f * w * w) + (2.0f * z * z) - 1.0f;
*roll = RADIANS_TO_DEGREES(atan2f(m23, m33));
*pitch = RADIANS_TO_DEGREES(asinf(-m13));
*yaw = RADIANS_TO_DEGREES(atan2f(m12, m11));
return 0;
}
// any other message types will just fall through and keep looping until the timeout is reached
}
else
return err; // return on timeouts or serious errors
return FREESPACE_ERROR_TIMEOUT; // The function returns gracefully without values
}
//-----------------------------------------------------------------------------
int GetOrientation(float* yaw, float* pitch, float* roll) {
int duration = 0;
int timeout = 250; // 1/4 second max time in this function
long start = clock();
freespace_message msg;
while (duration < timeout) {
int err = freespace_readMessage(DeviceID, &msg, timeout - duration);
if (err == 0) {
// Check if this is a user frame message.
if (msg.messageType == FREESPACE_MESSAGE_USERFRAME) {
// Convert from quaternion to Euler angles
// Get the quaternion vector
float w = msg.userFrame.angularPosA;
float x = msg.userFrame.angularPosB;
float y = msg.userFrame.angularPosC;
float z = msg.userFrame.angularPosD;
// normalize the vector
float len = sqrtf((w*w) + (x*x) + (y*y) + (z*z));
w /= len;
x /= len;
y /= len;
z /= len;
// The Freespace quaternion gives the rotation in terms of
// rotating the world around the object. We take the conjugate to
// get the rotation in the object's reference frame.
w = w;
x = -x;
y = -y;
z = -z;
// Convert to angles in radians
float m11 = (2.0f * w * w) + (2.0f * x * x) - 1.0f;
float m12 = (2.0f * x * y) + (2.0f * w * z);
float m13 = (2.0f * x * z) - (2.0f * w * y);
float m23 = (2.0f * y * z) + (2.0f * w * x);
float m33 = (2.0f * w * w) + (2.0f * z * z) - 1.0f;
if (bRawFeed) {
// Just return the raw samples
*roll = atan2f(m23, m33);
*pitch = asinf(-m13);
*yaw = atan2f(m12, m11);
return 0;
}
else {
// Run the new sample through the smoothing filter
// Find the index into the rotating sample window
CurSample++;
if (CurSample >= SMOOTHING_WINDOW_SIZE)
CurSample = 0;
// Add the new sample
RollWindow[CurSample] = atan2f(m23, m33);
PitchWindow[CurSample] = asinf(-m13);
YawWindow[CurSample] = atan2f(m12, m11);
if (NumSamples < SMOOTHING_WINDOW_SIZE) {
// At startup, don't return a value until the window is full
NumSamples++;
}
else {
// Average all the samples in the sample window
float roll_sum = RollWindow[CurSample];
float pitch_sum = PitchWindow[CurSample];
float yaw_sum = YawWindow[CurSample];
int i = CurSample+1;
if (i >= SMOOTHING_WINDOW_SIZE)
i = 0;
while (i != CurSample) {
roll_sum += RollWindow[i];
pitch_sum += PitchWindow[i];
if (fabs(YawWindow[CurSample] - YawWindow[i]) > PI) {
// If the yaw samples cross a discontinuity, then make the angles
// all positive or all negative so averaging works
if (YawWindow[CurSample] > 0)
yaw_sum += (YawWindow[i] + (2 * PI));
else
yaw_sum += (YawWindow[i] + (-2 * PI));
}
else
yaw_sum += YawWindow[i];
i++;
if (i >= SMOOTHING_WINDOW_SIZE)
i = 0;
}
// Compute the smoothed values
float next_roll = roll_sum / SMOOTHING_WINDOW_SIZE;
float next_pitch = pitch_sum / SMOOTHING_WINDOW_SIZE;
float next_yaw = yaw_sum / SMOOTHING_WINDOW_SIZE;
// correct yaw angles
if (next_yaw > PI)
next_yaw -= (2 * PI);
else if (next_yaw < -PI)
next_yaw += (2 * PI);
// Put a speed limit on pitch changes to minimize violent
// pitch recalibrations
if (Pitch == ANGLE_UNDEFINED) {
Pitch = next_pitch; // initial value
}
else {
// Limit any large angular changes
if (next_pitch < Pitch)
Pitch = max(next_pitch, Pitch - MAX_ANGULAR_CHANGE);
else if (next_pitch > Pitch)
Pitch = min(next_pitch, Pitch + MAX_ANGULAR_CHANGE);
}
// return the adjusted values
*roll = next_roll;
*pitch = Pitch;
*yaw = next_yaw;
return 0;
}
}
}
// any other message types will just fall through and keep looping until the timeout is reached
}
else
return err; // return on timeouts or serious errors
duration += clock() - start;
}
return FREESPACE_ERROR_TIMEOUT; // The function returns gracefully without values
}
/**
* main
* This example uses the synchronous API to
* - find a device
* - open the device found
* - send a message
* - look for a response
* This example assumes that the device is already connected.
*/
int main(int argc, char* argv[]) {
FreespaceDeviceId device; // Keep track of the device you are talking to
struct freespace_message send; // A place to create messages to send to the device
struct freespace_message receive; // A place to put a message received from the device
int numIds; // Keep track of how many devices are available
int rc; // Return Code
int retryCount = 0; // How many times tried so far to get a response
// Flag to indicate that the application should quit
// Set by the control signal handler
int quit = 0;
printVersionInfo(argv[0]);
addControlHandler(&quit);
// Initialize the freespace library
rc = freespace_init();
if (rc != FREESPACE_SUCCESS) {
printf("Initialization error. rc=%d\n", rc);
return 1;
}
printf("Scanning for Freespace devices...\n");
// Get the ID of the first device in the list of availble devices
rc = freespace_getDeviceList(&device, 1, &numIds);
if (numIds == 0) {
printf("Didn't find any devices.\n");
return 1;
}
printf("Found a device. Trying to open it...\n");
// Prepare to communicate with the device found above
rc = freespace_openDevice(device);
if (rc != FREESPACE_SUCCESS) {
printf("Error opening device: %d\n", rc);
return 1;
}
// Display the device information.
printDeviceInfo(device);
// Make sure any old messages are cleared out of the system
rc = freespace_flush(device);
if (rc != FREESPACE_SUCCESS) {
printf("Error flushing device: %d\n", rc);
return 1;
}
printf("Requesting battery level messages.\n");
memset(&send, 0, sizeof(send)); // Start with a clean message struct
// Populate the message fields. Two options are shown below. Uncomment one desired
// and comment out the one not desired.
//send.messageType = FREESPACE_MESSAGE_BATTERYLEVELREQUEST; // To send a battery level request
send.messageType = FREESPACE_MESSAGE_PRODUCTIDREQUEST; // To send a product ID request
while (!quit) {
if (retryCount < RETRY_COUNT_LIMIT) {
retryCount++;
// Send the message constructed above.
rc = freespace_sendMessage(device, &send);
if (rc != FREESPACE_SUCCESS) {
printf("Could not send message: %d.\n", rc);
}
// Read the response message.
rc = freespace_readMessage(device, &receive, 100);
if (rc == FREESPACE_SUCCESS) {
// Print the received message
freespace_printMessage(stdout, &receive);
retryCount = 0;
} else if (rc == FREESPACE_ERROR_TIMEOUT) {
printf("<timeout> Try moving the Freespace device to wake it up.\n");
} else if (rc == FREESPACE_ERROR_INTERRUPTED) {
printf("<interrupted>\n");
} else {
printf("Error reading: %d. Quitting...\n", rc);
break;
}
} else {
printf("Did not receive response after %d trials\n", RETRY_COUNT_LIMIT);
quit = 1;
}
SLEEP;
}
printf("Cleaning up...\n");
freespace_closeDevice(device);
freespace_exit();
return 0;
}
