Skip to content

Commit

Permalink
Initial implementation of quaternion reading and fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@r57zone
r57zone committed Jul 8, 2024
1 parent 547b748 commit 06d85cf
Show file tree
Hide file tree
Showing 3 changed files with 131 additions and 57 deletions.
154 changes: 115 additions & 39 deletions OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,16 +57,16 @@ inline void HmdMatrix_SetIdentity( HmdMatrix34_t *pMatrix )


// keys for use with the settings API
static const char * const k_pch_steamvr_Section = "steamvr";
static const char * const k_pch_arduinoHMD_SerialNumber_String = "serialNumber";
static const char * const k_pch_arduinoHMD_ModelNumber_String = "modelNumber";
static const char * const k_pch_arduinoHMD_WindowX_Int32 = "windowX";
static const char * const k_pch_arduinoHMD_WindowY_Int32 = "windowY";
static const char * const k_pch_steamvr_Section = "SteamVR";
static const char * const k_pch_arduinoHMD_SerialNumber_String = "SerialNumber";
static const char * const k_pch_arduinoHMD_ModelNumber_String = "ModelNumber";
static const char * const k_pch_arduinoHMD_WindowX_Int32 = "WindowX";
static const char * const k_pch_arduinoHMD_WindowY_Int32 = "WindowY";
static const char * const k_pch_arduinoHMD_WindowWidth_Int32 = "windowWidth";
static const char * const k_pch_arduinoHMD_WindowHeight_Int32 = "windowHeight";
static const char * const k_pch_arduinoHMD_RenderWidth_Int32 = "renderWidth";
static const char * const k_pch_arduinoHMD_RenderHeight_Int32 = "renderHeight";
static const char * const k_pch_arduinoHMD_SecondsFromVsyncToPhotons_Float = "secondsFromVsyncToPhotons";
static const char * const k_pch_arduinoHMD_RenderWidth_Int32 = "RenderWidth";
static const char * const k_pch_arduinoHMD_RenderHeight_Int32 = "RenderHeight";
static const char * const k_pch_arduinoHMD_SecondsFromVsyncToPhotons_Float = "SecondsFromVsyncToPhotons";
static const char * const k_pch_arduinoHMD_DisplayFrequency_Float = "displayFrequency";

// own output settings
Expand All @@ -75,30 +75,43 @@ static const char * const k_pch_arduinoHMD_DistortionK2_Float = "DistortionK2";
static const char * const k_pch_arduinoHMD_ZoomWidth_Float = "ZoomWidth";
static const char * const k_pch_arduinoHMD_ZoomHeight_Float = "ZoomHeight";
static const char * const k_pch_arduinoHMD_FOV_Float = "FOV";
static const char * const k_pch_arduinoHMD_IPD_Float = "IPD";
static const char * const k_pch_arduinoHMD_DistanceBetweenEyes_Int32 = "DistanceBetweenEyes";
static const char * const k_pch_arduinoHMD_ScreenOffsetX_Int32 = "ScreenOffsetX";
static const char * const k_pch_arduinoHMD_Stereo_Bool = "Stereo";
static const char * const k_pch_arduinoHMD_DebugMode_Bool = "DebugMode";

// arduino hmd settings
static const char * const k_pch_arduinoHMD_Section = "arduinohmd";
static const char * const k_pch_arduinoHMD_Section = "ArduinoHMD";
static const char * const k_pch_arduinoHMD_ArduinoRequire_Bool = "ArduinoRequire";
static const char * const k_pch_arduinoHMD_ArduinoRotationType_Bool = "RotationType";
static const char * const k_pch_arduinoHMD_COM_port_Int32 = "COMPort";
static const char * const k_pch_arduinoHMD_CenteringKey_String = "CenteringKey";
static const char * const k_pch_arduinoHMD_CrouchPressKey_String = "CrouchPressKey";
static const char * const k_pch_arduinoHMD_CrouchOffset_Float = "CrouchOffset";

HANDLE hSerial;
int32_t comPortNumber;

bool ArduinoRotationType = false; // 1 - quat, 0 - ypr
bool HMDConnected = false, HMDInitCentring = false, ArduinoNotRequire = false;
float ArduinoIMU[3] = { 0, 0, 0 }, yprOffset[3] = { 0, 0, 0 }; // Yaw, Pitch, Roll
float LastArduinoIMU[3] = { 0, 0, 0 };
std::thread *pArduinoReadThread = NULL;

// Position
double fPos[3] = { 0, 0, 0 };

#define StepPos 0.0033;
#define StepRot 0.2;

std::thread *pArduinoReadThread = NULL;
// YPR type
float ArduinoIMU[3] = { 0, 0, 0 }, yprOffset[3] = { 0, 0, 0 }; // Yaw, Pitch, Roll
float LastArduinoIMU[3] = { 0, 0, 0 };

// Quaternion type
struct QuaternionF {
float w, x, y, z;
};
QuaternionF ArduinoIMUQuat = { 0, 0, 0, 0 }, HMDQuatOffset = { 0, 0, 0, 0 }, LastArduinoIMUQuat = { 0, 0, 0, 0 };

double DegToRad(double f) {
return f * (3.14159265358979323846 / 180);
Expand Down Expand Up @@ -142,40 +155,94 @@ bool CorrectAngleValue(float Value)
return false;
}

bool CorrectQuatValue(float Value)
{
if (Value > 1 || Value < -1)
return false;
else
return true;
}

void SetCentering()
{
yprOffset[0] = ArduinoIMU[0];
yprOffset[1] = ArduinoIMU[1];
yprOffset[2] = ArduinoIMU[2];
if (ArduinoRotationType == false) {
yprOffset[0] = ArduinoIMU[0];
yprOffset[1] = ArduinoIMU[1];
yprOffset[2] = ArduinoIMU[2];
}
else {
float length = std::sqrt(ArduinoIMUQuat.x * ArduinoIMUQuat.x + ArduinoIMUQuat.y * ArduinoIMUQuat.y + ArduinoIMUQuat.z * ArduinoIMUQuat.z + ArduinoIMUQuat.w * ArduinoIMUQuat.w);
ArduinoIMUQuat.w /= length;
ArduinoIMUQuat.x /= length;
ArduinoIMUQuat.y /= length;
ArduinoIMUQuat.z /= length;
HMDQuatOffset.w = ArduinoIMUQuat.w;
HMDQuatOffset.x = ArduinoIMUQuat.x;
HMDQuatOffset.z = ArduinoIMUQuat.y;
HMDQuatOffset.z = ArduinoIMUQuat.z;
}
}

void ArduinoIMURead()
{
DWORD bytesRead;

while (HMDConnected) {
ReadFile(hSerial, &ArduinoIMU, sizeof(ArduinoIMU), &bytesRead, 0);

if (CorrectAngleValue(ArduinoIMU[0]) == false || CorrectAngleValue(ArduinoIMU[1]) == false || CorrectAngleValue(ArduinoIMU[2]) == false)
{
// last correct values
ArduinoIMU[0] = LastArduinoIMU[0];
ArduinoIMU[1] = LastArduinoIMU[1];
ArduinoIMU[2] = LastArduinoIMU[2];
// YPR
if (ArduinoRotationType == false) {
ReadFile(hSerial, &ArduinoIMU, sizeof(ArduinoIMU), &bytesRead, 0);

if (CorrectAngleValue(ArduinoIMU[0]) == false || CorrectAngleValue(ArduinoIMU[1]) == false || CorrectAngleValue(ArduinoIMU[2]) == false)
{
// last correct values
ArduinoIMU[0] = LastArduinoIMU[0];
ArduinoIMU[1] = LastArduinoIMU[1];
ArduinoIMU[2] = LastArduinoIMU[2];

PurgeComm(hSerial, PURGE_TXCLEAR | PURGE_RXCLEAR);
}
else if (CorrectAngleValue(ArduinoIMU[0]) && CorrectAngleValue(ArduinoIMU[1]) && CorrectAngleValue(ArduinoIMU[2])) // save last correct values
{
LastArduinoIMU[0] = ArduinoIMU[0];
LastArduinoIMU[1] = ArduinoIMU[1];
LastArduinoIMU[2] = ArduinoIMU[2];

if (HMDInitCentring == false)
if (ArduinoIMU[0] != 0 || ArduinoIMU[1] != 0 || ArduinoIMU[2] != 0) {
SetCentering();
HMDInitCentring = true;
}
}

// Quaternion
} else {
ReadFile(hSerial, &ArduinoIMUQuat, sizeof(ArduinoIMUQuat), &bytesRead, 0);

if (CorrectQuatValue(ArduinoIMUQuat.w) == false || CorrectQuatValue(ArduinoIMUQuat.x) == false || CorrectQuatValue(ArduinoIMUQuat.y) == false || CorrectQuatValue(ArduinoIMUQuat.z) == false)
{
// last correct values
ArduinoIMUQuat.w = LastArduinoIMUQuat.w;
ArduinoIMUQuat.x = LastArduinoIMUQuat.x;
ArduinoIMUQuat.y = LastArduinoIMUQuat.y;
ArduinoIMUQuat.z = LastArduinoIMUQuat.z;

PurgeComm(hSerial, PURGE_TXCLEAR | PURGE_RXCLEAR);
}
else if (CorrectQuatValue(ArduinoIMUQuat.w) && CorrectQuatValue(ArduinoIMUQuat.x) && CorrectQuatValue(ArduinoIMUQuat.y) && CorrectQuatValue(ArduinoIMUQuat.z)) // save last correct values
{
LastArduinoIMUQuat.w = ArduinoIMUQuat.w;
LastArduinoIMUQuat.x = ArduinoIMUQuat.x;
LastArduinoIMUQuat.y = ArduinoIMUQuat.y;
LastArduinoIMUQuat.z = ArduinoIMUQuat.z;

if (HMDInitCentring == false)
if (ArduinoIMUQuat.w != 0 || ArduinoIMUQuat.x != 0 || ArduinoIMUQuat.y != 0 || ArduinoIMUQuat.z != 0) {
SetCentering();
HMDInitCentring = true;
}
}

PurgeComm(hSerial, PURGE_TXCLEAR | PURGE_RXCLEAR);
}
else if (CorrectAngleValue(ArduinoIMU[0]) && CorrectAngleValue(ArduinoIMU[1]) && CorrectAngleValue(ArduinoIMU[2])) // save last correct values
{
LastArduinoIMU[0] = ArduinoIMU[0];
LastArduinoIMU[1] = ArduinoIMU[1];
LastArduinoIMU[2] = ArduinoIMU[2];

if (HMDInitCentring == false)
if (ArduinoIMU[0] != 0 || ArduinoIMU[1] != 0 || ArduinoIMU[2] != 0) {
SetCentering();
HMDInitCentring = true;
}
}

if (bytesRead == 0) Sleep(1);
Expand Down Expand Up @@ -344,7 +411,7 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl
m_ulPropertyContainer = vr::k_ulInvalidPropertyContainer;

//DriverLog( "Using settings values\n" );
m_flIPD = vr::VRSettings()->GetFloat(k_pch_steamvr_Section, k_pch_SteamVR_IPD_Float );
m_flIPD = vr::VRSettings()->GetFloat(k_pch_steamvr_Section, k_pch_arduinoHMD_IPD_Float);

char buf[1024];
vr::VRSettings()->GetString(k_pch_steamvr_Section, k_pch_arduinoHMD_SerialNumber_String, buf, sizeof( buf ) );
Expand Down Expand Up @@ -661,9 +728,17 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl
SetCentering();

// Set head tracking rotation
pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ),
DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ),
DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 ));
if (ArduinoRotationType == false) { // YPR
pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ),
DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ),
DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 ));
} else { // Quaternion
// Centered
pose.qRotation.w = ArduinoIMUQuat.w * LastArduinoIMUQuat.w - ArduinoIMUQuat.x * LastArduinoIMUQuat.x - ArduinoIMUQuat.y * LastArduinoIMUQuat.y - ArduinoIMUQuat.z * LastArduinoIMUQuat.z;
pose.qRotation.x = ArduinoIMUQuat.w * LastArduinoIMUQuat.x + ArduinoIMUQuat.x * LastArduinoIMUQuat.w + ArduinoIMUQuat.y * LastArduinoIMUQuat.z - ArduinoIMUQuat.z * LastArduinoIMUQuat.y;
pose.qRotation.y = ArduinoIMUQuat.w * LastArduinoIMUQuat.y - ArduinoIMUQuat.x * LastArduinoIMUQuat.z + ArduinoIMUQuat.y * LastArduinoIMUQuat.w + ArduinoIMUQuat.z * LastArduinoIMUQuat.x;
pose.qRotation.z = ArduinoIMUQuat.w * LastArduinoIMUQuat.z + ArduinoIMUQuat.x * LastArduinoIMUQuat.y - ArduinoIMUQuat.y * LastArduinoIMUQuat.x + ArduinoIMUQuat.z * LastArduinoIMUQuat.w;
}

//Set head position tracking
pose.vecPosition[0] = fPos[0]; // X
Expand Down Expand Up @@ -754,6 +829,7 @@ EVRInitError CServerDriver::Init( vr::IVRDriverContext *pDriverContext )
VR_INIT_SERVER_DRIVER_CONTEXT( pDriverContext );
//InitDriverLog( vr::VRDriverLog() );

ArduinoRotationType = vr::VRSettings()->GetInt32(k_pch_arduinoHMD_Section, k_pch_arduinoHMD_ArduinoRotationType_Bool);
ArduinoNotRequire = !vr::VRSettings()->GetInt32(k_pch_arduinoHMD_Section, k_pch_arduinoHMD_ArduinoRequire_Bool);
comPortNumber = vr::VRSettings()->GetInt32(k_pch_arduinoHMD_Section, k_pch_arduinoHMD_COM_port_Int32);

Expand Down
18 changes: 9 additions & 9 deletions README.RU.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,9 @@
Быстро включать и выключать расширенный VR монитор шлема можно, с помощью оболочки `HMD Assistant` или [MultiMonitorTool](https://www.nirsoft.net/utils/multi_monitor_tool.html).

## Arduino и трекеры вращения
Для отслеживания поворотов головы понадобится купить [Arduino Nano](http://ali.pub/2oy73f) и подключить к ней плату с датчиками вращения, например, [MPU 6050 GY-521](http://ali.pub/2oy76c), MPU 9250, MPU 3200 GY-85 или любую другую при наличии прошивки с выводом значений Yaw, Pitch, Roll (Рысканье, Тангаж, Крен), и калибровкой. Вывод данных происходит бинарный (3 float значения), пример можно посмотреть [тут](https://github.com/TrueOpenVR/TrueOpenVR-DIY/blob/master/HMD/Arduino/Arduino.Output.Bin.ino).
Подойдут любые COM трекеры с выводом 3 float значения yaw, pitch, roll или 4 float значения кватерниона w, x, y, z. Скорость вывода должна составлять.

Для отслеживания поворотов головы понадобится купить [Arduino Nano](http://ali.pub/2oy73f) и подключить к ней плату с датчиками вращения, например, [MPU 6050 GY-521](http://ali.pub/2oy76c), MPU 9250, MPU 3200 GY-85 или любую другую при наличии прошивки с выводом трёх float значений yaw, pitch, roll (рысканье, тангаж, крен) или 4 float значения кватерниона w, x, y, z и калибровкой. Вывод данных происходит бинарный (3 float или 4 float значения), пример можно посмотреть [тут](https://github.com/TrueOpenVR/TrueOpenVR-DIY/blob/master/HMD/Arduino/Arduino.Output.Bin.ino). При использовании кватерниона измените значение `RotationQuaternion` на `true`. Скорость вывода должна составлять `115200`.

Готовая прошивка Arduino есть для [MPU 3200 GY-85](http://alli.pub/5wxnyl), называется она [Razor AHRS](https://github.com/Razor-AHRS/razor-9dof-ahrs/tree/master/Arduino). Вместе с ней идет программа для калибровки и демонстрации. После калибровки замените файл "Output.ino", в папке с прошивкой, на [этот](https://github.com/TrueOpenVR/TrueOpenVR-DIY/blob/master/HMD/Arduino/Razor_AHRS/Output.ino).
Здесь важно отметить, что появились новые ревизии GY-85, которые несовместимы с этой прошивкой. Прошивкой поддерживаются следующие сенсоры: акселерометр ADXL345, гироскоп ITG-3200 и магнитометры HMC5843, HMC5883L. Инструкцию по калибровке можно найти на [youtube](https://www.youtube.com/watch?v=J7K_TnzQBZk).
Expand All @@ -29,25 +31,23 @@

![](https://user-images.githubusercontent.com/9499881/52521728-e200dc80-2c94-11e9-9628-68ea3ef3dacd.png)

Скорость должна быть изменена на `115200`, если по умолчанию задана другая.

## Параметры файла конфигурации
Название | Описание
------------ | -------------
COMPort | Номер COM порта Arduino, можно посмотреть в диспетчере устройств. Используйте порты от `1` до `9`, измените при необходимости его в свойствах устройства.
CenteringKey | Код кнопки центрирования изображения, изменить кнопку можно в файле конфигурации, вписав [нужное название кнопки](https://github.com/r57zone/DualShock4-emulator/blob/master/BINDINGS.RU.md)).
CrouchPressKey | Код кнопки приседания, изменить кнопку можно в файле конфигурации, вписав [нужное название кнопки](https://github.com/r57zone/DualShock4-emulator/blob/master/BINDINGS.RU.md)). Необходимо для связи с другими драйверами, например, используя контроллеры Razer Hydra и используя [этот драйвер](https://github.com/r57zone/Razer-Hydra-SteamVR-driver) можно приседать.
CrouchOffset | Высота приседания по нажатию кнопки.
FOV | Градус поля зрения. Можно увеличить, в зависимости от линз VR гарнитуры.
IPD | Межзрачковое расстояние.
DistanceBetweenEyes | Расстояние между стерео изображениями, чем больше, тем ближе.
DistortionK1, DistortionK2 | Коэффициенты искажения линз.
ScreenOffsetX | Сдвиг изображения по горизонтали.
ZoomHeight, ZoomWidth | Коэффициенты масштабирования стерео изображений.
FOV | Градус поля зрения. Можно увеличить, в зависимости от линз VR гарнитуры.
ipd | Межзрачковое расстояние.
displayFrequency | Частота обновления экрана.
renderWidth, renderHeight | Разрешение рендера изображения для одного глаза.
windowWidth, windowHeight | Высота и ширина выводимого окна.
windowX, windowY | Смещение окна, требуется для отображения на других мониторах (расширенных). Например, для отображения на втором дисплее, который отображается справа, нужно указать значение 1920 (при условии, что первый дисплей имеет разрешение 1920 на 1080). Точные данные можно просмотреть, с помощью [MultiMonitorTool утилиты](https://www.nirsoft.net/utils/multi_monitor_tool.html), которая также может выключать и включить второй монитор, через bat-файл.
DisplayFrequency | Частота обновления экрана.
RenderWidth, RenderHeight | Разрешение рендера изображения для одного глаза.
WindowWidth, WindowHeight | Высота и ширина выводимого окна.
WindowX, WindowY | Смещение окна, требуется для отображения на других мониторах (расширенных). Например, для отображения на втором дисплее, который отображается справа, нужно указать значение 1920 (при условии, что первый дисплей имеет разрешение 1920 на 1080). Точные данные можно просмотреть, с помощью [MultiMonitorTool утилиты](https://www.nirsoft.net/utils/multi_monitor_tool.html), которая также может выключать и включить второй монитор, через bat-файл.
DebugMode | Режим отладки, заблокирован на 30 FPS. Рекомендуется после проверки отключить (поставить `false`).
ArduinoRequire | Требование подключенного Arduino, для запуска драйвера. Параметр необходим для быстрых тестов контроллеров, чтобы не подключать и одевать шлем для тестов. Для отключения измените на `false`.

Expand Down
Loading

0 comments on commit 06d85cf

Please sign in to comment.