Les coulisses du Techdays 2014 : Développer un driver pour l’Oculus Rift : Part III
31 Janvier 2014 : J-10 avant l’édition des Techdays 2014. Dans mon dernier billet nous avons abordé la manière d’intégrer le driver aux Sensor APIS et de faire en sorte qu’il soit reconnu comme un driver de type OrientationSensor.
Dans ce billet, nous allons rentrer un peu plus dans le détail :
- de la manière dont les évènements sont retournés à l’appelant,
- comment formater les données pour qu’elles soit compréhensibles par les APis Sensor.
- enfin nous brancheront notre driver à l’Oculus Rift en utilisant les APIs du SDK Oculus Rift
Pour clarifier la situation, sur l’architecture du driver, voici une image grossière de son architecture
Rappelez-vous, la classe SensorManager est en charge :
- d’instancier la classe Sensor qui aura pour rôle :
- de faire le lien avec le capteur physique,
- de récupérer les données du capteur,
- de les convertir dans le bon format pour qu’elles soient compréhensibles par les APIs Sensor de Windows.
- d’instancier la classe SensorDDI(DDI =Device Driver Interface), qui a pour rôle :
- d’informer le Framework WDF des capacités, des propriétés et du type de driver (OrientationSensor),
- d’identifier lorsqu’un client se connecte/déconnecte, ou s’abonne/Désabonne à un évènement,
- de retourner des données au client.
- d’instancier la classe ISensorClassExtension fournit par Windows pour simplifier le développement de driver. Cette classe permet :
- de répondre au demande entrante (ProcessIoControl),
- de poster un évènement (PostEvent) lorsque des données sont disponibles,
- et de poster l’état du périphérique à un instant T (PosteStateChange)
- de démarrer un Thread qui sera à l’écoute, lorsque de nouvelles données seront disponibles.
Avant de démarrer le thread, la classe SensorManager, crée un HANDLE de type EVENT à l’aide de l’API CreateEvent.
if (SUCCEEDED(hr))
{
// Step 1: Create the Data Changed Event Handle
m_hSensorEvent = ::CreateEvent(NULL, // No security attributes
FALSE, // Automatic-reset event object
FALSE, // Initial state is non-signaled
NULL); // Unnamed object
if (m_pSensor != nullptr)
{
m_pSensor->SetDataEventHandle(m_hSensorEvent);
m_pSensor->m_fInitialDataReceived = FALSE;
}
else
{
hr = E_UNEXPECTED;
}
}
Le HANDLE sera utilisé, par la classe Sensor afin de signaler lorsque de nouvelles données sont disponibles.
En attendant, la classe SensorManager, crée un nouveau thread CreateThread, en lui passant comme paramètre la routine _SensorEventThread.
// Step 2: Activate & Create and start the eventing thread
if (SUCCEEDED(hr))
{
Activate();
m_hSensorManagerEventingThread = ::CreateThread(NULL, // Cannot be inherited by child process
0, // Default stack size
&SensorsManager::_SensorEventThreadProc, // Thread proc
(LPVOID)this, // Thread proc argument
0, // Starting state = running
NULL);
if (nullptr == m_hSensorManagerEventingThread)
{
TraceEvents(TRACE_LEVEL_ERROR, TRACE_DEVICE, "%!FUNC! sensor event thread failed to create");
hr = HRESULT_FROM_WIN32(::GetLastError());
}
else
{
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! sensor event thread successfully created");
}
if (SUCCEEDED(hr))
{
m_fSensorManagerInitialized = TRUE;
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! sensor initialization completed ");
}
else
{
TraceEvents(TRACE_LEVEL_ERROR, TRACE_DEVICE, "%!FUNC! sensor event initialization failed");
}
}
Pour attendre que l’EVENT, crée plus haut, soit signalé, la routine _SensorEventThreadProc, utilise l’API Windows WaitForSingleObject et se met en attente.
DWORD WINAPI SensorsManager::_SensorEventThreadProc(_In_ LPVOID pvData)
{
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! Entry");
SensorsManager* pParent = (SensorsManager*) pvData;
if (nullptr == pParent)
{
return 0;
}
SensorsManager* pThis = static_cast<SensorsManager*>(pvData);
HRESULT hr = CoInitializeEx(NULL, COINITBASE_MULTITHREADED);
if (FAILED(hr))
{
TraceEvents(TRACE_LEVEL_ERROR, TRACE_DEVICE, "Failed to call ConInitialize hr=%!HRESULT!", hr);
return 0;
}
// Create the event parameters collection if it doesn't exist
CComPtr<IPortableDeviceValues> spEventParams;
if (spEventParams == NULL)
{
hr = CoCreateInstance(CLSID_PortableDeviceValues,
NULL,
CLSCTX_INPROC_SERVER,
IID_IPortableDeviceValues,
(VOID**) &spEventParams);
}
if (FAILED(hr))
{
TraceEvents(TRACE_LEVEL_ERROR,TRACE_DEVICE, "Failed to CoCreateInstance for Event Parameters, hr = %!HRESULT!", hr);
return 0;
}
while (pParent->IsActive() &&
(WAIT_OBJECT_0 == WaitForSingleObject(pParent->GetSensorEventHandle(), INFINITE)))
{
hr = S_OK;
hr = pThis->EnterProcessing(PROCESSING_ISENSOREVENT);
if (S_OK == hr)
{
//initialize the event parameters
spEventParams->Clear();
//populate the event type
if (S_OK == hr)
{
Sensor *pSensor = pThis->GetDefaultSensor();
if (pSensor->HasValidDataEvent())
{
BOOL IsNewData=FALSE;
bool fStateChanged = FALSE;
hr = pParent->SetState(pSensor, SENSOR_STATE_READY, &fStateChanged);
if (SUCCEEDED(hr))
{
hr = pSensor->GetOrientationFromRift(spEventParams,&IsNewData);
}
if (SUCCEEDED(hr) && IsNewData==TRUE)
{
hr = spEventParams->SetGuidValue(SENSOR_EVENT_PARAMETER_EVENT_ID, SENSOR_EVENT_DATA_UPDATED);
pParent->PostDataEvent(pSensor->GetSensorObjectID(), spEventParams);
}
}
}
}
pThis->ExitProcessing(PROCESSING_ISENSOREVENT);
}
CoUninitialize();
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! Exit");
return 0;
}
Lorsque l’EVENT est signalé la routine démarre :
- prévient que le capteur est prêt, en changeant sont état de SENSOR_STATE_NO_DATA en SENSOR_STATE_READY.
L’appel de la méthode pParent->SetState() dans notre extrait de code ci-dessus, invoque la méthode PosteStateChange, de la classe ISensorClassExtension. - ensuite elle demande au Sensor, si de nouvelles données sont disponibles, en invoquant la méthode GetOrientationFromRift.
- si des données sont disponibles, on l’indique au client en postant les données avec la méthode PostDataEvent, qui invoque la méthode ISensorClassExtension::PosteEvent.
Maintenant on peut se poser la question, mais qui, comment et quand l’EVENT est passé à l’état signalé ?
Alors rappelez-vous encore, lorsqu’un client C# par exemple s’abonne à l’évènement de notre capteur comme illustré dans le code suivant :
private void ActivateOrientationSensor()
{
OrientationSensor sensor = OrientationSensor.GetDefault();
sensor.ReadingChanged += sensor_ReadingChanged;
}
void sensor_ReadingChanged(OrientationSensor sender, OrientationSensorReadingChangedEventArgs args)
{
var data = args.Reading;
}
Le Framework WDF, invoque la méthode, OnClientSubscribeToEvents, qui appellera la méthode de SetPollOrientationAsync de la classe Sensor, comme illustré dans le code suivant :
VOID Sensor::SetPollOrientationAsync()
{
m_hSimulateThread = ::CreateThread(NULL, // Cannot be inherited by child process
0, // Default stack size
&Sensor::_PollOrientationThreadProc, // Thread proc
(LPVOID)this, // Thread proc argument
0, // Starting state = running
NULL);
}
En faite, cette méthode, ne fait que démarrer un nouveau thread, qui exécutera la routine _PollOrientationThreadProc.
Note : Aujourd’hui, je ne suis pas hyper fier de cette méthode 
, car je fait du polling, mais pour un POC ça ira comme cela. Mais bon j’y travaille…
DWORD WINAPI Sensor::_PollOrientationThreadProc(_In_ LPVOID pvData)
{
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! Entry");
Sensor* pParent = (Sensor*) pvData;
if (nullptr == pParent)
{
return 0;
}
Sensor* pThis = static_cast<Sensor*>(pvData);
DWORD dwDataPollTimeLimit = pThis->GetReportInterval();
ULONGLONG ulCurrentEventTime = 0;
ULONG ulTimePassed = 0;
//TODO: See if it does not exist a better way than polling for data
do
{
ULONGLONG ullInitialEventTime = GetTickCount64();
do //spin here until dwDataPollTimeLimit has passed;
{
Yield();
//Does the CRI change ?
dwDataPollTimeLimit = pThis->GetReportInterval();
ulCurrentEventTime = GetTickCount64();
ulTimePassed = (ULONG) (ulCurrentEventTime - ullInitialEventTime); //time passed in milliseconds
} while ((ulTimePassed < dwDataPollTimeLimit));
pThis->RaiseDataEvent();
} while (pThis->m_OneSuscriber);
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! Exit");
return 0;
}
En quelques mots, on récupère le lapse de temps entre la levée des évènements GetReportInterval, si l’intervalle de temps est dépassé, alors on lève l’évènement RaiseDataEvent, qui invoque l’API Windows SetEvent afin de signaler l’EVENT qui réveille notre routine _SensorEventThreadProc.
VOID Sensor::RaiseDataEvent()
{
CComCritSecLock<CComAutoCriticalSection> scopeLock(m_CriticalSection);
// no wdk content
// Check if there are any subscribers
if (0 < m_OneSuscriber)
{
// Set the data event flag
m_fValidDataEvent = TRUE;
if (NULL != m_hSensorEvent)
{
SetEvent(m_hSensorEvent);
}
}
}
La méthode GetOrientationFromRift de la classe Sensor,est donc invoquée. Vous remarquerez, que nous appelons la méthode GetNewOrientation. Cetteméthode nous est fournit par la classe OculusRiftDevice, dont je n’ai pas encore parlé, mais qui est initialisée lors de l’instanciation de la classe Sensor et que j’ai défini juste pour séparer la plomberie, propre au Framework WDF, de la plomberie propre à l’Oculus Rift.
HRESULT Sensor::GetOrientationFromRift(IPortableDeviceValues* pValues, BOOL *pIsNewData)
{
CComCritSecLock<CComAutoCriticalSection> scopeLock(m_CriticalSection); // Make this call thread safe
const int MAX_FLOAT_FOR_QUATERNION = 4;
const int MAX_FLOAT_FOR_MATRIX3X3 = 9;
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! Enter");
HRESULT hr = S_OK;
//Get the orientation from the Oculus Rift SensorFusion
std::unique_ptr<FLOAT[]> q = std::unique_ptr<FLOAT[]>(new FLOAT[4]);
*pIsNewData = m_pOculusRiftDevice->GetNewOriention(m_CS, q.get());
FLOAT x = q[0];
FLOAT y = q[1];
FLOAT z = q[2];
FLOAT w = q[3];
FLOAT quaternion[] = { x, y, z, w };
PROPVARIANT varQuaternion = { 0 };
FLOAT xx = x*x;
FLOAT yy = y*y;
FLOAT zz = z*z;
FLOAT xy = x*y;
FLOAT zw = z*w;
FLOAT zx = z*x;
FLOAT yw = y*w;
FLOAT yz = y*z;
FLOAT xw = x*w;
FLOAT matrix[9];
matrix[0] = 1.0F - (2.0F*(yy + zz));
matrix[1] = 2.0F*(xy + zw);
matrix[2] = 2.0F*(zx - yw);
matrix[3] = 2.0F*(xy - zw);
matrix[4] = 1.0F - (2.0F*(zz + xx));
matrix[5] = 2.0F*(yz + xw);
matrix[6] = 2.0F*(zx + yw);
matrix[7] = 2.0F*(yz - xw);
matrix[8] = 1.0F - (2.0F*(yy + xx));
PROPVARIANT varMatrix = { 0 };
hr = InitPropVariantFromBuffer(quaternion, MAX_FLOAT_FOR_QUATERNION * sizeof (FLOAT), &varQuaternion);
if (SUCCEEDED(hr))
{
hr = pValues->SetValue(SENSOR_DATA_TYPE_QUATERNION, &varQuaternion);
}
if (SUCCEEDED(hr))
{
hr = InitPropVariantFromBuffer(matrix, MAX_FLOAT_FOR_MATRIX3X3 * sizeof(float), &varMatrix);
}
if (SUCCEEDED(hr))
{
hr = pValues->SetValue(SENSOR_DATA_TYPE_ROTATION_MATRIX, &varMatrix);
}
this->SetTimeStamp(pValues);
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_DEVICE, "%!FUNC! Exit hr = %!HRESULT!", hr);
return hr;
}
GetNewOrientation, retourne un tableau de FLOAT décrivant les 4 valeurs x, y, z, w du quaternion que nous devons retourner à l’appelant comme illustré sur la figure suivante :
Pour que notre tableau de FLOAT soit compréhensible par les APIs de plus haut niveau, il faut alors le convertir en un vecteur VT_VECTOR | VT_UI1, c’est à dire un tableau de bytes non signés.
Le modèle de programmation ATL/COM, utilise massivement la notion de VARIANT/PROPVARIANT, qui n’est ni plus ni moins, qu’une énorme structure dans laquelle on peu y mettre n’importe quoi et comme elle n’est jamais triviale à utiliser, il existe des “Helpers” pour nous simplifier la vie et ça tombe bien.
Dans le code ci dessus, j’utilise le Helper InitPropVariantFromBuffer pour initialiser la structure varQuaternion à partir du tableau de FLOAT qui nous est retourné.
Ensuite je l’ajoute à la collection pValues en lui précisant que c’est un quaternion, SENSOR_DATA_TYPE_QUATERNION.
Enfin la méthode SensorManager::PostDataEvent fera le nécessaire pour retourner les données au client.
La méthode GetNewOrientation, retourne vrai ou faux, si l’Oculus Rift à bougé ou pas, comme nous le verrons plus tard et ce drapeau sera utilisé par la routine _SensorEventThreadProc pour lever ou pas l’évènement ISensorClassExtension::PosteEvent.
La dernière partie consiste, maintenant à brancher le driver aux APIS de l’Oculus Rift.
La 1ere chose à faire, consiste à se connecter au site https://developer.oculusvr.com/ pour obtenir la documentation et le SDK de l’Oculus Rift.
Ensuite, lors de la connexion physique de l’Oculus Rift au PC, un tracker s’installe afin que les APIs de l’Oculus puisse se brancher dessus.
Ce tracker va nous simplifier grandement la vie, car du coup, il va fusionner les données provenant des trois capteurs (Gyroscope, Magnétomètre, Accéléromètre) intégrés aux lunettes. Pas besoin alors de complexifier le driver, qui devient de faite juste un driver de type Filter. Vous voyez du coup cela devient plus simple.
Après avoir intégré la librairie libovr64.lib, ou libovr32.lib (pas de version ARM), il faut :
- Initialiser la librairie LibOvr : OVR::System::Init() (Ne pas oublier de faire un OVR::System::Destroy() dans le destructeur de la classe)
- Créer une instance de la classe DeviceManager() : OVR::DeviceManager::Create(), afin de pouvoir lister les périphériques Oculus Disponibles DeviceManager::EnumerateDevices<OVR::SensorDevice>() (A ce stade un seul, mais il possible d’en gérer plusieurs à la fois).
- Ensuite, on va créer le périphérique lui même : OVR::Ptr<OVR::SensorDevice> m_pHardwareSensor= OVR::SensorDevice::Createdevice()
- Puis attacher une instance de la classe OVR::SensorFusion à notre périphérique :
OVR::SensorFusion::AttachToSensor(m_pHardwaresensor) . C’est cette classe SensorFusion, qui permettra de fusionner les données et de les retourner sous forme de Quaternion.
HRESULT OculusRiftDevice::Initialize(float accuracyorientation)
{
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_OCULUS, "%!FUNC! Entry");
HRESULT hr = S_OK;
if (m_fInitialized == FALSE)
{
m_accuracyOrientation = accuracyorientation; //TODO: This is not the good value
OVR::System::Init();
if (SUCCEEDED(hr))
{
//hr = GetConfigurationData(pWdfDevice); //TODO: See if the LIBOvr can provide the real information Data
m_pManager = *OVR::DeviceManager::Create();
}
if (m_pManager == nullptr)
{
hr = E_FAIL;
TraceEvents(TRACE_LEVEL_ERROR, TRACE_OCULUS, "Failed to create the DeviceManage");
}
OVR::DeviceEnumerator<OVR::SensorDevice> iSensor;
OVR::DeviceEnumerator<OVR::SensorDevice> oculusSensor;
if (SUCCEEDED(hr))
{
iSensor = m_pManager->EnumerateDevices<OVR::SensorDevice>();
if (!iSensor) //for this poc the device need to be plug-in when installating the driver
{
hr = E_FAIL;
TraceEvents(TRACE_LEVEL_ERROR, TRACE_OCULUS, "No sensor found");
}
}
if (SUCCEEDED(hr))
{
OVR::DeviceInfo di;
iSensor.GetDeviceInfo(&di);
if (strstr(di.ProductName, "Tracker"))
{
if (!oculusSensor) //only one sensor for this POC
oculusSensor = iSensor;
}
}
if (oculusSensor)
{
m_pHardwareSensor = *oculusSensor.CreateDevice();
if (!m_pHardwareSensor)
{
hr = E_FAIL;
TraceEvents(TRACE_LEVEL_ERROR, TRACE_OCULUS, "Could not create the sensor device");
}
if (SUCCEEDED(hr))
{
BOOL bSetRange=m_pHardwareSensor->SetRange(OVR::SensorRange(4 * 9.81f, 8 * OVR::Math<FLOAT>::Pi, 1.0f), TRUE);
if (bSetRange)
{
m_SensorFusion = std::unique_ptr<OVR::SensorFusion>(new OVR::SensorFusion());
BOOL bAttach = m_SensorFusion->AttachToSensor(m_pHardwareSensor);
if (!bAttach)
{
hr = E_FAIL;
}
}
else
{
hr = E_FAIL;
}
}
oculusSensor.Clear();
}
}
if (SUCCEEDED(hr))
{
m_fInitialized = TRUE;
}
TraceEvents(TRACE_LEVEL_INFORMATION, TRACE_OCULUS, "%!FUNC! Exit hr = %!HRESULT!", hr);
return hr;
}
La classe OVR::SensorFusion, possède une méthode GetOrientation, qui retourne un type OVR::Quatf, qui n’est ni plus ni moins qu’une classe qui possède entre autre 4 données membres publiques x, y, z, w, données que nous utiliserons, pour remplir un tableau de FLOAT, qui sera lui même plus tard converti en PROPVARIANT.
BOOL OculusRiftDevice::GetNewOriention(FLOAT cs, FLOAT *quaternion)
{
DOUBLE curtime = (OVR::Timer::GetTicks() - m_StartupTicks) * (1.0 / (DOUBLE) OVR::Timer::MksPerSecond);
#ifdef NO_FILTER
UNREFERENCED_PARAMETER(cs);
if ((curtime - m_LastUpdate) > m_accuracyOrientation)
{
m_LastUpdate = curtime;
OVR::Quatf q = m_SensorFusion->GetOrientation();
quaternion[0] = q.x;
quaternion[1] = q.y;
quaternion[2] = q.z;
quaternion[3] = q.w;
}
return quaternion;
#else
OVR::Quatf q;
Quaternion currentQ;
if ((curtime - m_LastUpdate) > m_accuracyOrientation)
{
m_LastUpdate = curtime;
//TODO: See if LIBOvr provide message Handler
q = m_SensorFusion->GetOrientation();
quaternion[0] = q.x;
quaternion[1] = q.y;
quaternion[2] = q.z;
quaternion[3] = q.w;
currentQ.x = q.x;
currentQ.y = q.y;
currentQ.z = q.z;
currentQ.w = q.w;
if (m_LastRaisedQuaternion.x == 0.0f &&
m_LastRaisedQuaternion.y == 0.0f &&
m_LastRaisedQuaternion.z == 0.0f &&
m_LastRaisedQuaternion.w == 0.0f)
{
m_LastRaisedQuaternion.x = currentQ.x;
m_LastRaisedQuaternion.y = currentQ.y;
m_LastRaisedQuaternion.z = currentQ.z;
m_LastRaisedQuaternion.w = currentQ.w;
return TRUE;
}
auto Offsetx = abs(currentQ.x - m_LastRaisedQuaternion.x);
auto Offsety = abs(currentQ.y - m_LastRaisedQuaternion.y);
auto Offsetz = abs(currentQ.z - m_LastRaisedQuaternion.z);
auto Offsetw = abs(currentQ.w - m_LastRaisedQuaternion.w);
m_LastRaisedQuaternion.x = currentQ.x;
m_LastRaisedQuaternion.y = currentQ.y;
m_LastRaisedQuaternion.z = currentQ.z;
m_LastRaisedQuaternion.w = currentQ.w;
if (Offsetx > cs || Offsety > cs || Offsetz > cs || Offsetw > cs)
{
return TRUE;
}
}
return FALSE;
#endif
}
Dans le code suivant, je calcule très simplement si l’utilisateur à bougé l’Oculus d’un angle supérieure à la valeur définie pour la propriété SENSOR_PROPERTY_CHANGE_SENSITIVITY. Si oui il faudra lever un évènement.
Les prochaines étapes consisteront, dans un 1er temps à optimiser le driver. D’ailleurs, en écrivant ce billet, j’ai pu voir certains points d’amélioration que je vais tenter
Ensuite, il faudra signer et faire valider le driver et ça c’est une autre paire de manches, que je vous conterai sans aucun doute.
A très bientôt et bon Techdays 2014 !
Eric