Skip to content

Commit 06d85cf

Browse files
r57zoner57zone
committed
Initial implementation of quaternion reading and fixes
1 parent 547b748 commit 06d85cf

File tree

3 files changed

+131
-57
lines changed

3 files changed

+131
-57
lines changed

‎OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp

Lines changed: 115 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -57,16 +57,16 @@ inline void HmdMatrix_SetIdentity( HmdMatrix34_t *pMatrix )
5757

5858

5959
// keys for use with the settings API
60-
static const char * const k_pch_steamvr_Section = "steamvr";
61-
static const char * const k_pch_arduinoHMD_SerialNumber_String = "serialNumber";
62-
static const char * const k_pch_arduinoHMD_ModelNumber_String = "modelNumber";
63-
static const char * const k_pch_arduinoHMD_WindowX_Int32 = "windowX";
64-
static const char * const k_pch_arduinoHMD_WindowY_Int32 = "windowY";
60+
static const char * const k_pch_steamvr_Section = "SteamVR";
61+
static const char * const k_pch_arduinoHMD_SerialNumber_String = "SerialNumber";
62+
static const char * const k_pch_arduinoHMD_ModelNumber_String = "ModelNumber";
63+
static const char * const k_pch_arduinoHMD_WindowX_Int32 = "WindowX";
64+
static const char * const k_pch_arduinoHMD_WindowY_Int32 = "WindowY";
6565
static const char * const k_pch_arduinoHMD_WindowWidth_Int32 = "windowWidth";
6666
static const char * const k_pch_arduinoHMD_WindowHeight_Int32 = "windowHeight";
67-
static const char * const k_pch_arduinoHMD_RenderWidth_Int32 = "renderWidth";
68-
static const char * const k_pch_arduinoHMD_RenderHeight_Int32 = "renderHeight";
69-
static const char * const k_pch_arduinoHMD_SecondsFromVsyncToPhotons_Float = "secondsFromVsyncToPhotons";
67+
static const char * const k_pch_arduinoHMD_RenderWidth_Int32 = "RenderWidth";
68+
static const char * const k_pch_arduinoHMD_RenderHeight_Int32 = "RenderHeight";
69+
static const char * const k_pch_arduinoHMD_SecondsFromVsyncToPhotons_Float = "SecondsFromVsyncToPhotons";
7070
static const char * const k_pch_arduinoHMD_DisplayFrequency_Float = "displayFrequency";
7171

7272
// own output settings
@@ -75,30 +75,43 @@ static const char * const k_pch_arduinoHMD_DistortionK2_Float = "DistortionK2";
7575
static const char * const k_pch_arduinoHMD_ZoomWidth_Float = "ZoomWidth";
7676
static const char * const k_pch_arduinoHMD_ZoomHeight_Float = "ZoomHeight";
7777
static const char * const k_pch_arduinoHMD_FOV_Float = "FOV";
78+
static const char * const k_pch_arduinoHMD_IPD_Float = "IPD";
7879
static const char * const k_pch_arduinoHMD_DistanceBetweenEyes_Int32 = "DistanceBetweenEyes";
7980
static const char * const k_pch_arduinoHMD_ScreenOffsetX_Int32 = "ScreenOffsetX";
8081
static const char * const k_pch_arduinoHMD_Stereo_Bool = "Stereo";
8182
static const char * const k_pch_arduinoHMD_DebugMode_Bool = "DebugMode";
8283

8384
// arduino hmd settings
84-
static const char * const k_pch_arduinoHMD_Section = "arduinohmd";
85+
static const char * const k_pch_arduinoHMD_Section = "ArduinoHMD";
8586
static const char * const k_pch_arduinoHMD_ArduinoRequire_Bool = "ArduinoRequire";
87+
static const char * const k_pch_arduinoHMD_ArduinoRotationType_Bool = "RotationType";
8688
static const char * const k_pch_arduinoHMD_COM_port_Int32 = "COMPort";
8789
static const char * const k_pch_arduinoHMD_CenteringKey_String = "CenteringKey";
8890
static const char * const k_pch_arduinoHMD_CrouchPressKey_String = "CrouchPressKey";
8991
static const char * const k_pch_arduinoHMD_CrouchOffset_Float = "CrouchOffset";
9092

9193
HANDLE hSerial;
9294
int32_t comPortNumber;
95+
96+
bool ArduinoRotationType = false; // 1 - quat, 0 - ypr
9397
bool HMDConnected = false, HMDInitCentring = false, ArduinoNotRequire = false;
94-
float ArduinoIMU[3] = { 0, 0, 0 }, yprOffset[3] = { 0, 0, 0 }; // Yaw, Pitch, Roll
95-
float LastArduinoIMU[3] = { 0, 0, 0 };
98+
std::thread *pArduinoReadThread = NULL;
99+
100+
// Position
96101
double fPos[3] = { 0, 0, 0 };
97102

98103
#define StepPos 0.0033;
99104
#define StepRot 0.2;
100105

101-
std::thread *pArduinoReadThread = NULL;
106+
// YPR type
107+
float ArduinoIMU[3] = { 0, 0, 0 }, yprOffset[3] = { 0, 0, 0 }; // Yaw, Pitch, Roll
108+
float LastArduinoIMU[3] = { 0, 0, 0 };
109+
110+
// Quaternion type
111+
struct QuaternionF {
112+
float w, x, y, z;
113+
};
114+
QuaternionF ArduinoIMUQuat = { 0, 0, 0, 0 }, HMDQuatOffset = { 0, 0, 0, 0 }, LastArduinoIMUQuat = { 0, 0, 0, 0 };
102115

103116
double DegToRad(double f) {
104117
return f * (3.14159265358979323846 / 180);
@@ -142,40 +155,94 @@ bool CorrectAngleValue(float Value)
142155
return false;
143156
}
144157

158+
bool CorrectQuatValue(float Value)
159+
{
160+
if (Value > 1 || Value < -1)
161+
return false;
162+
else
163+
return true;
164+
}
165+
145166
void SetCentering()
146167
{
147-
yprOffset[0] = ArduinoIMU[0];
148-
yprOffset[1] = ArduinoIMU[1];
149-
yprOffset[2] = ArduinoIMU[2];
168+
if (ArduinoRotationType == false) {
169+
yprOffset[0] = ArduinoIMU[0];
170+
yprOffset[1] = ArduinoIMU[1];
171+
yprOffset[2] = ArduinoIMU[2];
172+
}
173+
else {
174+
float length = std::sqrt(ArduinoIMUQuat.x * ArduinoIMUQuat.x + ArduinoIMUQuat.y * ArduinoIMUQuat.y + ArduinoIMUQuat.z * ArduinoIMUQuat.z + ArduinoIMUQuat.w * ArduinoIMUQuat.w);
175+
ArduinoIMUQuat.w /= length;
176+
ArduinoIMUQuat.x /= length;
177+
ArduinoIMUQuat.y /= length;
178+
ArduinoIMUQuat.z /= length;
179+
HMDQuatOffset.w = ArduinoIMUQuat.w;
180+
HMDQuatOffset.x = ArduinoIMUQuat.x;
181+
HMDQuatOffset.z = ArduinoIMUQuat.y;
182+
HMDQuatOffset.z = ArduinoIMUQuat.z;
183+
}
150184
}
151185

152186
void ArduinoIMURead()
153187
{
154188
DWORD bytesRead;
155189

156190
while (HMDConnected) {
157-
ReadFile(hSerial, &ArduinoIMU, sizeof(ArduinoIMU), &bytesRead, 0);
158191

159-
if (CorrectAngleValue(ArduinoIMU[0]) == false || CorrectAngleValue(ArduinoIMU[1]) == false || CorrectAngleValue(ArduinoIMU[2]) == false)
160-
{
161-
// last correct values
162-
ArduinoIMU[0] = LastArduinoIMU[0];
163-
ArduinoIMU[1] = LastArduinoIMU[1];
164-
ArduinoIMU[2] = LastArduinoIMU[2];
192+
// YPR
193+
if (ArduinoRotationType == false) {
194+
ReadFile(hSerial, &ArduinoIMU, sizeof(ArduinoIMU), &bytesRead, 0);
195+
196+
if (CorrectAngleValue(ArduinoIMU[0]) == false || CorrectAngleValue(ArduinoIMU[1]) == false || CorrectAngleValue(ArduinoIMU[2]) == false)
197+
{
198+
// last correct values
199+
ArduinoIMU[0] = LastArduinoIMU[0];
200+
ArduinoIMU[1] = LastArduinoIMU[1];
201+
ArduinoIMU[2] = LastArduinoIMU[2];
202+
203+
PurgeComm(hSerial, PURGE_TXCLEAR | PURGE_RXCLEAR);
204+
}
205+
else if (CorrectAngleValue(ArduinoIMU[0]) && CorrectAngleValue(ArduinoIMU[1]) && CorrectAngleValue(ArduinoIMU[2])) // save last correct values
206+
{
207+
LastArduinoIMU[0] = ArduinoIMU[0];
208+
LastArduinoIMU[1] = ArduinoIMU[1];
209+
LastArduinoIMU[2] = ArduinoIMU[2];
210+
211+
if (HMDInitCentring == false)
212+
if (ArduinoIMU[0] != 0 || ArduinoIMU[1] != 0 || ArduinoIMU[2] != 0) {
213+
SetCentering();
214+
HMDInitCentring = true;
215+
}
216+
}
217+
218+
// Quaternion
219+
} else {
220+
ReadFile(hSerial, &ArduinoIMUQuat, sizeof(ArduinoIMUQuat), &bytesRead, 0);
221+
222+
if (CorrectQuatValue(ArduinoIMUQuat.w) == false || CorrectQuatValue(ArduinoIMUQuat.x) == false || CorrectQuatValue(ArduinoIMUQuat.y) == false || CorrectQuatValue(ArduinoIMUQuat.z) == false)
223+
{
224+
// last correct values
225+
ArduinoIMUQuat.w = LastArduinoIMUQuat.w;
226+
ArduinoIMUQuat.x = LastArduinoIMUQuat.x;
227+
ArduinoIMUQuat.y = LastArduinoIMUQuat.y;
228+
ArduinoIMUQuat.z = LastArduinoIMUQuat.z;
229+
230+
PurgeComm(hSerial, PURGE_TXCLEAR | PURGE_RXCLEAR);
231+
}
232+
else if (CorrectQuatValue(ArduinoIMUQuat.w) && CorrectQuatValue(ArduinoIMUQuat.x) && CorrectQuatValue(ArduinoIMUQuat.y) && CorrectQuatValue(ArduinoIMUQuat.z)) // save last correct values
233+
{
234+
LastArduinoIMUQuat.w = ArduinoIMUQuat.w;
235+
LastArduinoIMUQuat.x = ArduinoIMUQuat.x;
236+
LastArduinoIMUQuat.y = ArduinoIMUQuat.y;
237+
LastArduinoIMUQuat.z = ArduinoIMUQuat.z;
238+
239+
if (HMDInitCentring == false)
240+
if (ArduinoIMUQuat.w != 0 || ArduinoIMUQuat.x != 0 || ArduinoIMUQuat.y != 0 || ArduinoIMUQuat.z != 0) {
241+
SetCentering();
242+
HMDInitCentring = true;
243+
}
244+
}
165245

166-
PurgeComm(hSerial, PURGE_TXCLEAR | PURGE_RXCLEAR);
167-
}
168-
else if (CorrectAngleValue(ArduinoIMU[0]) && CorrectAngleValue(ArduinoIMU[1]) && CorrectAngleValue(ArduinoIMU[2])) // save last correct values
169-
{
170-
LastArduinoIMU[0] = ArduinoIMU[0];
171-
LastArduinoIMU[1] = ArduinoIMU[1];
172-
LastArduinoIMU[2] = ArduinoIMU[2];
173-
174-
if (HMDInitCentring == false)
175-
if (ArduinoIMU[0] != 0 || ArduinoIMU[1] != 0 || ArduinoIMU[2] != 0) {
176-
SetCentering();
177-
HMDInitCentring = true;
178-
}
179246
}
180247

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

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

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

663730
// Set head tracking rotation
664-
pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ),
665-
DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ),
666-
DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 ));
731+
if (ArduinoRotationType == false) { // YPR
732+
pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ),
733+
DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ),
734+
DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 ));
735+
} else { // Quaternion
736+
// Centered
737+
pose.qRotation.w = ArduinoIMUQuat.w * LastArduinoIMUQuat.w - ArduinoIMUQuat.x * LastArduinoIMUQuat.x - ArduinoIMUQuat.y * LastArduinoIMUQuat.y - ArduinoIMUQuat.z * LastArduinoIMUQuat.z;
738+
pose.qRotation.x = ArduinoIMUQuat.w * LastArduinoIMUQuat.x + ArduinoIMUQuat.x * LastArduinoIMUQuat.w + ArduinoIMUQuat.y * LastArduinoIMUQuat.z - ArduinoIMUQuat.z * LastArduinoIMUQuat.y;
739+
pose.qRotation.y = ArduinoIMUQuat.w * LastArduinoIMUQuat.y - ArduinoIMUQuat.x * LastArduinoIMUQuat.z + ArduinoIMUQuat.y * LastArduinoIMUQuat.w + ArduinoIMUQuat.z * LastArduinoIMUQuat.x;
740+
pose.qRotation.z = ArduinoIMUQuat.w * LastArduinoIMUQuat.z + ArduinoIMUQuat.x * LastArduinoIMUQuat.y - ArduinoIMUQuat.y * LastArduinoIMUQuat.x + ArduinoIMUQuat.z * LastArduinoIMUQuat.w;
741+
}
667742

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

832+
ArduinoRotationType = vr::VRSettings()->GetInt32(k_pch_arduinoHMD_Section, k_pch_arduinoHMD_ArduinoRotationType_Bool);
757833
ArduinoNotRequire = !vr::VRSettings()->GetInt32(k_pch_arduinoHMD_Section, k_pch_arduinoHMD_ArduinoRequire_Bool);
758834
comPortNumber = vr::VRSettings()->GetInt32(k_pch_arduinoHMD_Section, k_pch_arduinoHMD_COM_port_Int32);
759835

‎README.RU.md

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,9 @@
1818
Быстро включать и выключать расширенный VR монитор шлема можно, с помощью оболочки `HMD Assistant` или [MultiMonitorTool](https://www.nirsoft.net/utils/multi_monitor_tool.html).
1919

2020
## Arduino и трекеры вращения
21-
Для отслеживания поворотов головы понадобится купить [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).
21+
Подойдут любые COM трекеры с выводом 3 float значения yaw, pitch, roll или 4 float значения кватерниона w, x, y, z. Скорость вывода должна составлять.
22+
23+
Для отслеживания поворотов головы понадобится купить [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`.
2224

2325
Готовая прошивка 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).
2426
Здесь важно отметить, что появились новые ревизии GY-85, которые несовместимы с этой прошивкой. Прошивкой поддерживаются следующие сенсоры: акселерометр ADXL345, гироскоп ITG-3200 и магнитометры HMC5843, HMC5883L. Инструкцию по калибровке можно найти на [youtube](https://www.youtube.com/watch?v=J7K_TnzQBZk).
@@ -29,25 +31,23 @@
2931

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

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

0 commit comments

Comments
 (0)