src/Fusion.cpp

//###################################################################################
//#                                                               SpacePoint Fusion Object                                                                                #
//#                                                       Used To Handle a SpacePoint Fusion device                                                           #
//#                                                                       Author :                                                                            #
//#                                                                   Aymeric SUTEAU                                                                                    #
//#                                                                    LISA - ANGERS                                                                                    #
//###################################################################################


/*-------------------------------- HEADER INCLUDE ---------------------------------*/
#include "Plugin.h"


/*-------------------------- GLOBAL VARIABLES FOR HID  ----------------------------*/
HIDP_CAPS Capabilities;
PSP_DEVICE_INTERFACE_DETAIL_DATA detailData;
HANDLE DeviceHandle, WriteHandle, hEventObject, hDevInfo, hEventObjectWrite, hEventObjectRead;
GUID HidGuid;
OVERLAPPED HIDOverlapped, HIDOverlappedWrite, HIDOverlappedRead;
ULONG Length, Required;
DWORD NumberOfBytesRead, Result;
unsigned char InputReport[21], OutputReport[256];
bool MyDeviceDetected = FALSE;
int nbReadBytes = 0;


/*-------------------------- CONSTRUCTOR AND DESTRUCTOR ---------------------------*/
Fusion::Fusion() : Thread() 
{
  fAcceleration = new float[3];
  iButton = new int[2];
  iRawAxes = new int[11];

  SetConnected(0);                  // Device not connected yet
  SetNbSamples(-1);                 // Number of maximum samples not defined
  SetVID(FUSION_VID);               // Define SpacePoint Fusion VID
  SetPID(FUSION_PID);               // Define SpacePoint Fusion PID
  SetDataType(ORIENTATION_DATA);    // Define data type we want to retrieve (RAW or ORIENTATION)

  // Thread handling
  this->start();
  this->isRunning = true;
  this->bStatus = false;
}

Fusion::~Fusion() 
{  
  // Thread handling
  this->isRunning = false;
  this->bStatus = false;
  this->stop();

  delete [] fAcceleration;
  delete [] iButton;
  delete [] iRawAxes;
}


/*------------------------------ GETTERS AND SETTERS ------------------------------*/
bool Fusion::GetConnected() 
{
  return this->bConnected;
}

void Fusion::SetConnected(bool status) 
{
  this->bConnected = status;
}

int Fusion::GetNbSamples() 
{
  return this->iNbSamples;
}

void Fusion::SetNbSamples(int nbSamples) 
{
  this->iNbSamples = nbSamples;
}

int Fusion::GetVID() 
{
  return this->iVID;
}

void Fusion::SetVID(int VID) 
{
  this->iVID = VID;
}

int Fusion::GetPID()
{
  return this->iPID;
}

void Fusion::SetPID(int PID) 
{
  this->iPID = PID;
}

int Fusion::GetDataType() 
{
  return this->iDataType;
}

void Fusion::SetDataType(int dataType) 
{
  this->iDataType = dataType;
}


/*--------------------------------- OTHER METHODS ---------------------------------*/
bool Fusion::OpenDevice(int dataType) 
{
  // Set data type we want to retrieve (raw or orientation mode)
  SetDataType(dataType);

  // Try to connect to SpacePoint Fusion device
  SetConnected(FindHID(FUSION_VID, FUSION_PID, GetDataType()));
  if (!GetConnected())
  {
    return false;
  }
  else 
  {
    bStatus = true;
    return true;
  }
}

void Fusion::CloseDevice() 
{
  SetConnected(false);

  // Close handle to the HID device
  if (DeviceHandle != INVALID_HANDLE_VALUE)
    CloseHandle(DeviceHandle);

  // Update connection status for the thread
  bStatus = false;
}

// Interface : 0 = raw data, Interface 1 = Kalman filter output (meaningful orientation data)
bool Fusion::ParseData()
{
  int nbBytes = 0, i = 0, byteIndex = 1;

  // Device opened ?
  if (GetConnected() != 0) 
  {
    // Data read ?
    if (nbBytes = ReadDataFromHID() > 0)
    {
      // ------------ "Raw data" mode : retrieve raw data from Fusion device (accelerometer, gyroscope, and magnetometer data)
      if (GetDataType() == 0) 
      {
        // Get first raw values
        for (i=0; i<9; i++) 
        {
                            iRawAxes[i] = (int)InputReport[byteIndex] + 256*(int)InputReport[byteIndex + 1];
                            byteIndex += 2;
                    }   

        // Get buttons values
        iButton[0] = InputReport[byteIndex] & 1;
                    iButton[1] = (InputReport[byteIndex] >> 1) & 1;

        // Get final raw values
                    iRawAxes[9] = (InputReport[byteIndex] >> 4) & 0xf;
                    byteIndex++;
                    iRawAxes[10] = InputReport[byteIndex];

        // Notify Scol window that new raw data (acceleration and quaternion) has been read
        PostMessage(HScol, FUSION_RAW_DATA_CB, (int)this, (LPARAM)NULL);

        // Eventually notify Scol window if a button has been pressed
        if (iButton[0] == 1 || iButton[1] == 1)
          PostMessage(HScol, FUSION_BUTTON_CB, (int)this, (LPARAM)NULL);
      }

      // ------------ Mode "Orientation data" : retrieve calculated orientation information (acceleration and quaternions)
      else if (GetDataType() == 1) 
      {
        // Get first raw values
                    for (i=0; i<7; i++) 
        {
          iRawAxes[i] = (int)InputReport[byteIndex] + 256*(int)InputReport[byteIndex+1];
                            byteIndex += 2;
                    }

        // Get buttons values
                    iButton[0] = InputReport[byteIndex]&1;
                    iButton[1] = (InputReport[byteIndex]>>1)&1;

        // Get final raw values
                    iRawAxes[7] = (InputReport[byteIndex] >> 4) & 0xf;

        // 16 bit raw values centered at 32768
                    // acc_scaled = 6 * (acc_received - 32768)/32768;
                    for(i=0; i<3; i++)
                            fAcceleration[i] = 6.0f * (iRawAxes[i] - 32768) / 32768.0f;

                    // 16 bit raw values centered at 32768
                    // q_scaled = 3.0518e - 005 * (qraw - 32768)
        //TODO find the good quaternion conversion
        fQuaternion = Quaternion(3.0518e-005f * (iRawAxes[6] - 32768), 
                                 3.0518e-005f * (iRawAxes[4] - 32768),
                                 3.0518e-005f * (iRawAxes[5] - 32768), 
                                 3.0518e-005f * (iRawAxes[3] - 32768));
        
        // Invert quaternion on the Y axis (initial shift is 180 degrees)
        fQuaternion = fQuaternion * Quaternion(0, 0, 1, 0);

        // Notify Scol window that new orientation data has been read
        PostMessage(HScol, FUSION_ORIENTATION_DATA_CB, (int)this, (LPARAM)NULL);

        // Eventually notify Scol window if a button has been pressed
        if (iButton[0] == 1 || iButton[1] == 1)
          PostMessage(HScol, FUSION_BUTTON_CB, (int)this, (LPARAM)NULL);
      }

      // Data successfully read from device
      return true;
    }
    else
      return false;   // Unable to read data from device
  }
  else
    return false;
}


/*------------------------------- USB HID HANDLING --------------------------------*/
bool Fusion::FindHID(unsigned int vendorID, unsigned int productID, unsigned int interfaceMode)
{
        // Use a series of API calls to find a HID with a specified Vendor IF and Product ID
        HIDD_ATTRIBUTES                 Attributes;
        SP_DEVICE_INTERFACE_DATA                        devInfoData;
        bool                                                            LastDevice = FALSE;
        int                                                                     MemberIndex = 0;
        LONG                                                            Result;

        Length = 0;
        detailData = NULL;
        DeviceHandle = NULL;

        /*
        API function: HidD_GetHidGuid
        Get the GUID for all system HIDs.
        Returns: the GUID in HidGuid.
        */
        HidD_GetHidGuid(&HidGuid);
        
        /*
        API function: SetupDiGetClassDevs
        Returns: a handle to a device information set for all installed devices.
        Requires: the GUID returned by GetHidGuid.
        */
        hDevInfo = SetupDiGetClassDevs(&HidGuid,
                                             NULL,
                                             NULL,
                                             DIGCF_PRESENT|DIGCF_INTERFACEDEVICE);
                
        devInfoData.cbSize = sizeof(devInfoData);

        // Step through the available devices looking for the one we want.
        // Quit on detecting the desired device or checking all available devices without success
        MemberIndex = 0;
        LastDevice = FALSE;
        do
        {
                /*
                API function: SetupDiEnumDeviceInterfaces
                On return, MyDeviceInterfaceData contains the handle to a SP_DEVICE_INTERFACE_DATA structure for a detected device.
                Requires:
                  The DeviceInfoSet returned in SetupDiGetClassDevs.
                  The HidGuid returned in GetHidGuid.
                  An index to specify a device.
                */
                Result = SetupDiEnumDeviceInterfaces(hDevInfo, 
                                                           0, 
                                                           &HidGuid, 
                                                           MemberIndex, 
                                                           &devInfoData);

                if (Result != 0)
                {
                        // A device has been detected, so get more information about it
                        /*
                        API function: SetupDiGetDeviceInterfaceDetail
                        Returns: an SP_DEVICE_INTERFACE_DETAIL_DATA structure containing information about a device.
                        To retrieve the information, call this function twice:
                          The first time returns the size of the structure in Length.
                          The second time returns a pointer to the data in DeviceInfoSet.
                        Requires:
                        A DeviceInfoSet returned by SetupDiGetClassDevs
                        The SP_DEVICE_INTERFACE_DATA structure returned by SetupDiEnumDeviceInterfaces.
                        
                        The final parameter is an optional pointer to an SP_DEV_INFO_DATA structure.
                        This application doesn't retrieve or use the structure.                 
                        If retrieving the structure, set MyDeviceInfoData.cbSize = length of MyDeviceInfoData, and pass the structure's address.
                        */
                        // Get the Length value
                        // The call will return with a "buffer too small" error which can be ignored
                        Result = SetupDiGetDeviceInterfaceDetail(hDevInfo, 
                                                                       &devInfoData, 
                                                                       NULL, 
                                                                       0, 
                                                                       &Length, 
                                                                       NULL);

                        // Allocate memory for the hDevInfo structure, using the returned Length
                        detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA) malloc(Length);
                        
                        // Set cbSize in the detailData structure
                        detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);

                        // Call the function again, this time passing it the returned buffer size
                        Result = SetupDiGetDeviceInterfaceDetail(hDevInfo, 
                                                                       &devInfoData, 
                                                                       detailData, 
                                                                       Length, 
                                                                       &Required, 
                                                                       NULL);

                        // Open a handle to the device
                        // To enable retrieving information about a system mouse or keyboard, don't request Read or Write access for this handle
                        /*
                        API function: CreateFile
                        Returns: a handle that enables reading and writing to the device.
                        Requires: The DevicePath in the detailData structure returned by SetupDiGetDeviceInterfaceDetail.
                        */
                        DeviceHandle = CreateFile(detailData->DevicePath, 
                                                        GENERIC_READ|GENERIC_WRITE, //0, 
                                                        FILE_SHARE_READ|FILE_SHARE_WRITE, 
                                                        (LPSECURITY_ATTRIBUTES)NULL,
                                                        OPEN_EXISTING, 
                                                        FILE_FLAG_OVERLAPPED, //0, 
                                                        NULL);

                        /*
                        API function: HidD_GetAttributes
                        Requests information from the device.
                        Requires: the handle returned by CreateFile.
                        Returns: a HIDD_ATTRIBUTES structure containing the Vendor ID, Product ID, and Product Version Number.
                        Use this information to decide if the detected device is the one we're looking for.
                        */
                        // Set the Size to the number of bytes in the structure
                        Attributes.Size = sizeof(Attributes);
                        Result = HidD_GetAttributes(DeviceHandle, 
                                                          &Attributes);
                        
                        // Is it the desired device?
                        MyDeviceDetected = FALSE;
                        
      // Check Vendor ID (must be 'SpacePoint')
                        if (Attributes.VendorID == vendorID)
                        {
        // Check Product ID (must be 'Fusion')
        if (Attributes.ProductID == productID)
                                {
                                        // Both the Vendor ID and Product ID match, we can get device capabilities to check if interface mode is correct
                                        MyDeviceDetected = TRUE;

                                        // Register to receive device notifications
                                        RegisterForDeviceNotifications();

                                        // Get the device's capablities
                                        GetDeviceCapabilities();

          // Check interface mode (0=raw and 1=orientation)
          if (Capabilities.UsagePage != interfaceMode)
          {
            // Interface mode is not correct, so it's not the desired device
            printf(">>> Wrong interface mode.\n");
            MyDeviceDetected = FALSE;
                                          CloseHandle(DeviceHandle);
          }
          else
          {
            // Send the request command using WriteFile
            hEventObjectWrite = CreateEvent(NULL,
                                            TRUE,
                                            TRUE,
                                            NULL);
            HIDOverlappedWrite.hEvent = hEventObjectWrite;
            HIDOverlappedWrite.Offset = 0;
            HIDOverlappedWrite.OffsetHigh = 0;
            Result = WriteFile(DeviceHandle,
                               &OutputReport[0],
                               Capabilities.OutputReportByteLength,
                               &NumberOfBytesRead,
                               (LPOVERLAPPED)&HIDOverlapped);
          }
                                }
                                else
                                        CloseHandle(DeviceHandle);  // Wrong Product ID
                        }
                        else
                                CloseHandle(DeviceHandle);  // Wrong Vendor ID

                  // Free the memory used by the detailData structure (no longer needed)
                  free(detailData);
                }
                else
                        //SetupDiEnumDeviceInterfaces returned 0, so there are no more devices to check
                        LastDevice = TRUE;

          //If we haven't found the device yet, and haven't tried every available device, try the next one
          MemberIndex = MemberIndex + 1;
        }
  while ((LastDevice == FALSE) && (MyDeviceDetected == FALSE));

  // Create log message
        if (MyDeviceDetected == FALSE)
                printf("Device not detected\n");
        else
                printf("Device detected\n");

        // Free the memory reserved for hDevInfo by SetupDiClassDevs
        SetupDiDestroyDeviceInfoList(hDevInfo);

  // Return device detection status
        return MyDeviceDetected;
}

void Fusion::RegisterForDeviceNotifications()
{
        // Request to receive messages when a device is attached or removed
        // Also see WM_DEVICECHANGE in BEGIN_MESSAGE_MAP(CUsbhidiocDlg, CDialog)
        DEV_BROADCAST_DEVICEINTERFACE DevBroadcastDeviceInterface;
        HDEVNOTIFY DeviceNotificationHandle;

        DevBroadcastDeviceInterface.dbcc_size = sizeof(DevBroadcastDeviceInterface);
        DevBroadcastDeviceInterface.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE;
        DevBroadcastDeviceInterface.dbcc_classguid = HidGuid;

        DeviceNotificationHandle = RegisterDeviceNotification(NULL,   // TODO: Handle that will catch messages (Scol window handle ?)
                                                        &DevBroadcastDeviceInterface,
                                                        DEVICE_NOTIFY_WINDOW_HANDLE);
}

void Fusion::GetDeviceCapabilities()
{
        // Get the Capabilities structure for the device
        PHIDP_PREPARSED_DATA    PreparsedData;

        /*
        API function: HidD_GetPreparsedData
        Returns: a pointer to a buffer containing the information about the device's capabilities.
        Requires: A handle returned by CreateFile.
        There's no need to access the buffer directly, but HidP_GetCaps and other API functions require a pointer to the buffer.
        */
        HidD_GetPreparsedData(DeviceHandle, &PreparsedData);

        /*
        API function: HidP_GetCaps
        Learn the device's capabilities.
        For standard devices such as joysticks, you can find out the specific capabilities of the device.
        For a custom device, the software will probably know what the device is capable of, and the call only verifies the information.
        Requires: the pointer to the buffer returned by HidD_GetPreparsedData.
        Returns: a Capabilities structure containing the information.
        */
        HidP_GetCaps(PreparsedData, &Capabilities);

        // No need for PreparsedData any more, so free the memory it's using
        HidD_FreePreparsedData(PreparsedData);
}

int Fusion::ReadDataFromHID()
{
  // Send the request command using ReadFile
  hEventObjectRead = CreateEvent(NULL,
                                 TRUE,
                                 TRUE,
                                 NULL);
  HIDOverlappedRead.hEvent = hEventObjectRead;
  HIDOverlappedRead.Offset = 0;
  HIDOverlappedRead.OffsetHigh = 0;
  Result = ReadFile(DeviceHandle,
                    &InputReport[0],
                    Capabilities.InputReportByteLength,
                    &NumberOfBytesRead,
                    (LPOVERLAPPED)&HIDOverlappedRead);         
  Result = WaitForSingleObject(hEventObjectRead, 600);
  DWORD dw = GetLastError();
  return dw;
}

/*-------------------------------- THREAD HANDLING --------------------------------*/
void Fusion::run() 
{
  try
  {
    while(isRunning)
    {
      // Parse data only if device has been opened
      if (bStatus)
        ParseData();  // ORIENTATION_DATA or RAW_DATA
    }
    // Release connection to the SpacePoint Fusion device
    CloseDevice();
  }
  catch (ThreadException ex) 
  {
    MMechostr(MSKDEBUG, "error thread : %s\n", ex.getMessage().c_str());
  }             
}