int freespace_sendMessageAsync(FreespaceDeviceId id,
struct freespace_message* message,
unsigned int timeoutMs,
freespace_sendCallback callback,
void* cookie) {
int rc;
uint8_t msgBuf[FREESPACE_MAX_OUTPUT_MESSAGE_SIZE];
struct FreespaceDeviceInfo info;
// Address is reserved for now and must be set to 0 by the caller.
if (message->dest == 0) {
message->dest = FREESPACE_RESERVED_ADDRESS;
}
rc = freespace_getDeviceInfo(id, &info);
if (rc != FREESPACE_SUCCESS) {
return rc;
}
message->ver = info.hVer;
rc = freespace_encode_message(message, msgBuf, FREESPACE_MAX_OUTPUT_MESSAGE_SIZE);
if (rc <= FREESPACE_SUCCESS) {
return rc;
}
return freespace_private_sendAsync(id, msgBuf, rc, timeoutMs, callback, cookie);
}
LIBFREESPACE_API int freespace_sendMessage(FreespaceDeviceId id,
struct freespace_message* message) {
int retVal;
uint8_t msgBuf[FREESPACE_MAX_OUTPUT_MESSAGE_SIZE];
struct FreespaceDeviceInfo info;
memset(msgBuf, 0, sizeof(msgBuf));
// Address is reserved for now and must be set to 0 by the caller.
if (message->dest == 0) {
message->dest = FREESPACE_RESERVED_ADDRESS;
}
retVal = freespace_getDeviceInfo(id, &info);
if (retVal != FREESPACE_SUCCESS) {
return retVal;
}
message->ver = info.hVer;
retVal = freespace_encode_message(message, msgBuf, FREESPACE_MAX_OUTPUT_MESSAGE_SIZE);
if (retVal <= FREESPACE_SUCCESS) {
return retVal;
}
return freespace_private_send(id, msgBuf, retVal);
}
int FreeSpaceTracker::connect()
{
OutputDebugString("Free Tracker Connect\n");
// Initialize the freespace library
int err = freespace_init();
if (err)
return err;
// Look for a single tracker
int num_devices;
err = freespace_getDeviceList(&DeviceID, 1, &num_devices);
if (err)
return err;
if (num_devices == 0)
return FREESPACE_ERROR_NO_DEVICE;
// Attach to the tracker
err = freespace_openDevice(DeviceID);
if (err)
{
DeviceID = -1;
return err;
}
err = freespace_getDeviceInfo(DeviceID, &Device);
// Flush the message stream
err = freespace_flush(DeviceID);
if (err)
return err;
// Turn on the orientation message stream
freespace_message msg;
memset(&msg, 0, sizeof(msg));
if (Device.hVer >= 2)
{
// version 2 protocol
msg.messageType = FREESPACE_MESSAGE_DATAMODECONTROLV2REQUEST;
msg.dataModeControlV2Request.packetSelect = 3; // User Frame (orientation)
msg.dataModeControlV2Request.modeAndStatus = 0; // operating mode == full motion
}
else
{
// version 1 protocol
msg.messageType = FREESPACE_MESSAGE_DATAMODEREQUEST;
msg.dataModeRequest.enableUserPosition = 1;
msg.dataModeRequest.inhibitPowerManager = 1;
}
err = freespace_sendMessage(DeviceID, &msg);
if (err)
return err;
// Now the tracker is ready to read
return 0;
}
int freespace_readMessage(FreespaceDeviceId id,
struct freespace_message* message,
unsigned int timeoutMs) {
int rc;
uint8_t buffer[FREESPACE_MAX_INPUT_MESSAGE_SIZE];
int actLen;
struct FreespaceDeviceInfo info;
rc = freespace_getDeviceInfo(id, &info);
if (rc != FREESPACE_SUCCESS) {
return rc;
}
rc = freespace_private_read(id, buffer, sizeof(buffer), timeoutMs, &actLen);
if (rc == FREESPACE_SUCCESS) {
return freespace_decode_message(buffer, actLen, message, info.hVer);
} else {
return rc;
}
}
vrpn_Freespace* vrpn_Freespace::create(const char *name, vrpn_Connection *conn,
int device_index,
bool send_body_frames, bool send_user_frames)
{
// initialize libfreespace
vrpn_Freespace::freespaceInit();
const unsigned MAX_DEVICES = 100;
FreespaceDeviceId devices[MAX_DEVICES];
int numIds = 0;
int rc;
rc = freespace_getDeviceList(devices, MAX_DEVICES, &numIds);
if ( (rc != FREESPACE_SUCCESS) || (numIds < (device_index+1)) ) {
fprintf(stderr, "vrpn_Freespace::create: Didn't find enough devices: %d.\n", numIds);
return NULL;
}
FreespaceDeviceId freespaceId = devices[device_index];
rc = freespace_openDevice(freespaceId);
if (rc != FREESPACE_SUCCESS) {
fprintf(stderr, "vrpn_Freespace::create: Could not open device %d.\n", device_index);
return NULL;
}
struct FreespaceDeviceInfo deviceInfo;
rc = freespace_getDeviceInfo(freespaceId, &deviceInfo);
if (rc != FREESPACE_SUCCESS) {
return NULL;
}
rc = freespace_flush(freespaceId);
if (rc != FREESPACE_SUCCESS) {
fprintf(stderr, "vrpn_Freespace::create: Error flushing device: %d\n", rc);
return NULL;
}
vrpn_Freespace* dev = new vrpn_Freespace(freespaceId, &deviceInfo, name, conn);
dev->deviceSetConfiguration(send_body_frames, send_user_frames);
printf("Added freespace device: %s : \n", name);
return dev;
}
/**
* Callback that
* - displays the devices that have been inserted into and removed from the system.
* - requests the firmware version of any devices that have been inserted into the system.
* Implements freespace_hotplugCallback
* @param event The type of event.
* @param id The affected device.
* @param cookie Not used for this application.
*/
void hotplugCallback(enum freespace_hotplugEvent event,
FreespaceDeviceId id,
void* cookie) {
int rc;
struct freespace_message message;
struct FreespaceDeviceInfo info;
struct deviceInfo* iter;
struct deviceInfo* del;
switch (event) {
case FREESPACE_HOTPLUG_INSERTION:
// Get and print USB HID information about the device.
printf("Device Inserted: %d\n", id);
rc = printDeviceInfo(id);
if (rc != FREESPACE_SUCCESS) {
printf("Could not display device info: %d\n", rc);
return;
}
// Open the device to be able to communicate with it
rc = freespace_openDevice(id);
if (rc != FREESPACE_SUCCESS) {
printf("Error opening device.\n");
return;
}
rc = freespace_getDeviceInfo(id, &info);
if (rc != FREESPACE_SUCCESS) {
printf(" Error getting device info. Error code %d\n", rc);
return;
}
printf(" HID Version = %d\n", info.hVer);
// Set the handler that handles messages received from the device.
freespace_setReceiveMessageCallback(id, receiveMessageCallback, NULL);
// Create and send a product ID request message to the device
memset(&message, 0, sizeof(message)); // Start with a clean message struct
message.messageType = FREESPACE_MESSAGE_PRODUCTIDREQUEST;
// Allow message.dest to have the default value of 0. This will cause the message
// to go to the remote or the module
rc = freespace_sendMessageAsync(id, &message, 100, NULL, 0);
if (rc != FREESPACE_SUCCESS) {
printf("Error sending productID request\n");
return;
}
// To communicate with a dongle set the dest field to 1
message.dest = 1;
rc = freespace_sendMessageAsync(id, &message, 100, NULL, 0);
if (rc != FREESPACE_SUCCESS) {
printf("Error sending productID request\n");
return;
}
break;
case FREESPACE_HOTPLUG_REMOVAL:
printf("Device Removed: %d\n", id);
if (deviceInfoList != NULL) {
for(iter = deviceInfoList; iter->next != NULL; iter = iter->next) {
if (iter->id == id) { // It is the head
deviceInfoList = iter->next;
free(iter);
break;
} else if (iter->next != NULL) { // Not the head, in the list
if (iter->next->id == id) {
del = iter->next;
iter->next = iter->next->next;
free(del);
break;
}
} else { // Not in the list
}
}
}
break;
default:
printf("Unrecognized freespace_hotplugEvent.\n");
break;
}
}
