mirror of
https://github.com/lbgos/rift-s-linux.git
synced 2026-10-27 14:01:30 +01:00
d/rift_sensor: Add sensor initialization code
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2748>
This commit is contained in:
Beyley Cardellio authored and Marge Bot committed
1 parent
a325708657
commit
cf441bd4e6
10 files changed
+1425
-1
No files matched your search
+4
-1
@@ -332,7 +332,7 @@ option_with_deps(XRT_HAVE_LIBUSB "Enable libusb (used for most drivers)" DEPENDS
|
||||
option_with_deps(XRT_HAVE_LIBUVC "Enable libuvc video driver" DEPENDS LIBUVC_FOUND XRT_HAVE_LIBUSB)
|
||||
option_with_deps(XRT_HAVE_REALSENSE "Enable RealSense support" DEPENDS realsense2_FOUND)
|
||||
option_with_deps(XRT_HAVE_OPENVR "Enable OpenVR support" DEPENDS OPENVR_FOUND)
|
||||
option_with_deps(XRT_BUILD_DRIVER_UVC "Enable userspace UVC driver" OFF DEPENDS XRT_HAVE_LIBUSB)
|
||||
option_with_deps(XRT_BUILD_DRIVER_UVC "Enable userspace UVC driver" DEPENDS XRT_HAVE_LIBUSB)
|
||||
|
||||
# Drivers to build (sorted)
|
||||
option_with_deps(XRT_BUILD_DRIVER_ANDROID "Enable Android sensors driver" DEPENDS ANDROID)
|
||||
@@ -357,6 +357,7 @@ option_with_deps(XRT_BUILD_DRIVER_QWERTY "Enable Qwerty driver" DEPENDS XRT_HAVE
|
||||
option_with_deps(XRT_BUILD_DRIVER_REALSENSE "Enable RealSense device driver" DEPENDS XRT_HAVE_REALSENSE)
|
||||
option_with_deps(XRT_BUILD_DRIVER_REMOTE "Enable remote debugging driver" DEPENDS "XRT_HAVE_LINUX OR ANDROID OR WIN32")
|
||||
option_with_deps(XRT_BUILD_DRIVER_RIFT "Enable Oculus Rift (DK2/CV1) device driver" DEPENDS XRT_HAVE_INTERNAL_HID)
|
||||
option_with_deps(XRT_BUILD_DRIVER_RIFT_SENSOR "Enable Oculus Rift Sensor (DK2/CV1) device driver" DEPENDS XRT_BUILD_DRIVER_RIFT XRT_BUILD_DRIVER_UVC XRT_HAVE_LIBUSB)
|
||||
option_with_deps(XRT_BUILD_DRIVER_RIFT_S "Enable Oculus Rift S device driver" DEPENDS XRT_HAVE_V4L2)
|
||||
option_with_deps(XRT_BUILD_DRIVER_ROKID "Enable Rokid driver" DEPENDS "XRT_HAVE_LIBUSB")
|
||||
option_with_deps(XRT_BUILD_DRIVER_STEAMVR_LIGHTHOUSE "Enable SteamVR Lighthouse driver" DEPENDS XRT_HAVE_LINUX XRT_MODULE_AUX_VIVE)
|
||||
@@ -467,6 +468,7 @@ list(
|
||||
"REALSENSE"
|
||||
"REMOTE"
|
||||
"RIFT"
|
||||
"RIFT_SENSOR"
|
||||
"RIFT_S"
|
||||
"ROKID"
|
||||
"SURVIVE"
|
||||
@@ -715,6 +717,7 @@ message(STATUS "# DRIVER_QWERTY: ${XRT_BUILD_DRIVER_QWERTY}")
|
||||
message(STATUS "# DRIVER_REALSENSE: ${XRT_BUILD_DRIVER_REALSENSE}")
|
||||
message(STATUS "# DRIVER_REMOTE: ${XRT_BUILD_DRIVER_REMOTE}")
|
||||
message(STATUS "# DRIVER_RIFT: ${XRT_BUILD_DRIVER_RIFT}")
|
||||
message(STATUS "# DRIVER_RIFT_SENSOR: ${XRT_BUILD_DRIVER_RIFT_SENSOR}")
|
||||
message(STATUS "# DRIVER_RIFT_S: ${XRT_BUILD_DRIVER_RIFT_S}")
|
||||
message(STATUS "# DRIVER_ROKID: ${XRT_BUILD_DRIVER_ROKID}")
|
||||
message(STATUS "# DRIVER_SIMULATED: ${XRT_BUILD_DRIVER_SIMULATED}")
|
||||
|
||||
@@ -310,6 +310,33 @@ if(XRT_BUILD_DRIVER_RIFT)
|
||||
list(APPEND ENABLED_HEADSET_DRIVERS rift)
|
||||
endif()
|
||||
|
||||
if(XRT_BUILD_DRIVER_RIFT_SENSOR)
|
||||
add_library(
|
||||
drv_rift_sensor STATIC
|
||||
rift_sensor/ar0134.c
|
||||
rift_sensor/ar0134.h
|
||||
rift_sensor/esp570.c
|
||||
rift_sensor/esp570.h
|
||||
rift_sensor/esp770u.c
|
||||
rift_sensor/esp770u.h
|
||||
rift_sensor/mt9v034.c
|
||||
rift_sensor/mt9v034.h
|
||||
)
|
||||
target_include_directories(drv_rift_sensor SYSTEM PRIVATE)
|
||||
target_link_libraries(
|
||||
drv_rift_sensor
|
||||
PRIVATE
|
||||
xrt-interfaces
|
||||
aux_util
|
||||
aux_util_sink
|
||||
aux_math
|
||||
drv_includes
|
||||
drv_uvc
|
||||
${LIBUSB1_LIBRARIES}
|
||||
)
|
||||
target_include_directories(drv_rift_sensor PUBLIC ${LIBUSB1_INCLUDE_DIRS})
|
||||
endif()
|
||||
|
||||
if(XRT_BUILD_DRIVER_RIFT_S)
|
||||
add_library(
|
||||
drv_rift_s STATIC
|
||||
|
||||
@@ -0,0 +1,308 @@
|
||||
// Copyright 2017, Philipp Zabel
|
||||
// Copyright 2019-2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of ar0134 sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#include "os/os_time.h"
|
||||
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include "uvc/uvc_interface.h"
|
||||
|
||||
#include "ar0134.h"
|
||||
|
||||
|
||||
#define XU_ENTITY 4
|
||||
#define SENSOR_REG_SEL 2
|
||||
#define PIXEL_CLOCKS_PER_LINE 1388
|
||||
#define COARSE_INTEGRATION_TIME 26
|
||||
#define FINE_INTEGRATION_TIME 646
|
||||
|
||||
/*
|
||||
* Internal functions
|
||||
*/
|
||||
|
||||
static int
|
||||
ar0134_read_reg(libusb_device_handle *dev, uint16_t reg, uint16_t *val)
|
||||
{
|
||||
uint8_t buf[6] = {0x86, 0x20, reg >> 8, reg & 0xff, 0x00, 0x00};
|
||||
int ret;
|
||||
|
||||
ret = uvc_set_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, sizeof(buf));
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = uvc_get_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, 3);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (buf[0] != 0x86) {
|
||||
U_LOG_D("(0x%x): %02x %02x %02x", reg, buf[0], buf[1], buf[2]);
|
||||
}
|
||||
*val = (buf[2] << 8) | buf[1];
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int
|
||||
ar0134_write_reg(libusb_device_handle *dev, uint16_t reg, uint16_t val)
|
||||
{
|
||||
uint8_t buf[64] = {0x06, 0x20, reg >> 8, reg & 0xff, val >> 8, val & 0xff};
|
||||
int ret;
|
||||
|
||||
ret = uvc_set_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, 6);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = uvc_get_cur(dev, 0, XU_ENTITY, SENSOR_REG_SEL, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (buf[0] != 0x06 || buf[1] != 0x20 || buf[2] != (reg >> 8) || buf[3] != (reg & 0xff) ||
|
||||
buf[4] != (val >> 8) || buf[5] != (val & 0xff)) {
|
||||
U_LOG_D("(0x%04x, 0x%04x): %02x %02x %02x %02x %02x %02x", reg, val, buf[0], buf[1], buf[2], buf[3],
|
||||
buf[4], buf[5]);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
* Exported functions
|
||||
*/
|
||||
|
||||
int
|
||||
rift_sensor_ar0134_init(libusb_device_handle *devh, bool usb2_mode)
|
||||
{
|
||||
uint16_t val;
|
||||
int ret;
|
||||
|
||||
#if 0 // Debug: dump all registers
|
||||
for (int i = 0x3000; i < 0x3100; i += 2) {
|
||||
if (i % 16 == 0)
|
||||
printf("%04x: ", i);
|
||||
ret = ar0134_read_reg(devh, i, &val);
|
||||
if (ret < 0) return ret;
|
||||
printf("%04x ", val);
|
||||
if (i % 16 == 14)
|
||||
printf("\n");
|
||||
}
|
||||
#endif
|
||||
|
||||
// Read chip version and revision number registers
|
||||
for (int i = 0; i < 10; i++) {
|
||||
ret = ar0134_read_reg(devh, 0x3000, &val);
|
||||
if (ret < 0 || val != 0x2406) {
|
||||
// 5000us sleep between retries
|
||||
os_nanosleep(OS_NS_PER_USEC * 5000);
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x2406) {
|
||||
U_LOG_E("This is not an AR0134 sensor: 0x%04x", val);
|
||||
}
|
||||
|
||||
ret = ar0134_read_reg(devh, 0x300e, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x1300) {
|
||||
U_LOG_W("Unexpected revision number: 0x%04x", val);
|
||||
}
|
||||
|
||||
ret = ar0134_read_reg(devh, 0x30b0, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x0080) {
|
||||
U_LOG_W("Expected monochrome mode instead of 0x%04x", val);
|
||||
}
|
||||
|
||||
// Disable embedded register data and statistics. For now we can't use this anyway.
|
||||
ret = ar0134_read_reg(devh, 0x3064, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x1882 && val != 0x1982) {
|
||||
U_LOG_W("Unexpected embedded data control: %04x", val);
|
||||
}
|
||||
ret = ar0134_write_reg(devh, 0x3064, val & ~0x0180);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Set data pedestal (black level) to zero
|
||||
ret = ar0134_write_reg(devh, 0x301e, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Disable binning
|
||||
ret = ar0134_write_reg(devh, 0x3032, 0x0000);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Set all gain values to the default, in USB2 mode 0x0007 is used instead.
|
||||
ret = ar0134_write_reg(devh, 0x305e, usb2_mode ? 0x0007 : 0x0020);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// This changes nothing, probably clear some already cleared bits.
|
||||
ret = ar0134_read_reg(devh, 0x30b0, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
val = 0x0080; // val &= ~(?);
|
||||
ret = ar0134_write_reg(devh, 0x30b0, val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Set vertical and horizontal capture ranges to maximum (0-959, 0-1279).
|
||||
ret = ar0134_write_reg(devh, 0x3002, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = ar0134_write_reg(devh, 0x3006, 959);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = ar0134_write_reg(devh, 0x3004, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = ar0134_write_reg(devh, 0x3008, 1279);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Set minimum supported pixel clocks per line
|
||||
ret = ar0134_write_reg(devh, 0x300c, PIXEL_CLOCKS_PER_LINE);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Set a short line bit, needed for the 1388 pclk line length above.
|
||||
ret = ar0134_read_reg(devh, 0x30b0, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
val |= (1 << 10);
|
||||
ret = ar0134_write_reg(devh, 0x30b0, val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Set total number of lines, 37 lines vertical blanking
|
||||
ret = ar0134_write_reg(devh, 0x300a, 997);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read number of pixel clocks per line, should be changed above.
|
||||
ret = ar0134_read_reg(devh, 0x300c, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (val != PIXEL_CLOCKS_PER_LINE) {
|
||||
U_LOG_E("Too many pixel clocks per line: %u, wanted %u", val, PIXEL_CLOCKS_PER_LINE);
|
||||
}
|
||||
|
||||
// Set coarse integration time (in multiples of lines) and
|
||||
// fine integration time (in multiples of the pixel clock).
|
||||
ret = ar0134_write_reg(devh, 0x3012, COARSE_INTEGRATION_TIME); // 26 lines
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = ar0134_write_reg(devh, 0x3014, FINE_INTEGRATION_TIME);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = ar0134_read_reg(devh, 0x3012, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != COARSE_INTEGRATION_TIME) {
|
||||
U_LOG_E("Failed to set coarse integration time: %u", val);
|
||||
}
|
||||
ret = ar0134_read_reg(devh, 0x3014, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != FINE_INTEGRATION_TIME) {
|
||||
U_LOG_E("Failed to set fine integration time: %u", val);
|
||||
}
|
||||
|
||||
// Stop streaming, trigger external exposure from nRF51288
|
||||
ret = ar0134_read_reg(devh, 0x301a, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x10dc) {
|
||||
U_LOG_W("Unexpected reset register value: 0x%04x", val);
|
||||
}
|
||||
// Force PLL always enabled
|
||||
val |= 1 << 11;
|
||||
// Enable trigger input pin
|
||||
val |= 1 << 8;
|
||||
// Disable streaming (switch to externally triggered mode)
|
||||
val &= ~(1 << 2);
|
||||
ret = ar0134_write_reg(devh, 0x301a, val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Enable junction temperature sensor
|
||||
ret = ar0134_read_reg(devh, 0x30b4, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
// Temperature sensor power
|
||||
val |= 1 << 0;
|
||||
// Start conversion
|
||||
val |= 1 << 4;
|
||||
ret = ar0134_write_reg(devh, 0x30b4, val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read 70 °C calibration point
|
||||
uint16_t calib_70c;
|
||||
ret = ar0134_read_reg(devh, 0x30c6, &calib_70c);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read 50 °C calibration point
|
||||
uint16_t calib_50c;
|
||||
ret = ar0134_read_reg(devh, 0x30c8, &calib_50c);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read temperature sensor
|
||||
ret = ar0134_read_reg(devh, 0x30b2, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
U_LOG_D("Temperature: %.1f °C", 50.0 + 20.0 * (val - calib_50c) / (calib_70c - calib_50c));
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
// Copyright 2017, Philipp Zabel
|
||||
// Copyright 2019-2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of ar0134 sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "xrt/xrt_defines.h"
|
||||
|
||||
#include <libusb.h>
|
||||
|
||||
|
||||
/*!
|
||||
* Sensor setup after stream start
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] usb2_mode Whether to configure for USB2 mode
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
rift_sensor_ar0134_init(libusb_device_handle *devh, bool usb2_mode);
|
||||
@@ -0,0 +1,157 @@
|
||||
// Copyright 2014-2020, Philipp Zabel
|
||||
// Copyright 2017, TheOnlyJoey
|
||||
// Copyright 2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of esp570 sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author TheOnlyJoey <joeyferweda@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include "uvc/uvc_interface.h"
|
||||
|
||||
#include "esp570.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
|
||||
|
||||
#define ESP570_EXTENSION_UNIT 4
|
||||
|
||||
#define ESP570_SELECTOR_I2C 2
|
||||
#define ESP570_SELECTOR_UNKNOWN_3 3
|
||||
#define ESP570_SELECTOR_EEPROM 5
|
||||
|
||||
/*
|
||||
* Internal functions
|
||||
*/
|
||||
|
||||
/*!
|
||||
* Calls SET_CUR and then GET_CUR on a given selector of the eSP570 UVC
|
||||
* extension unit.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] selector Selector to call on
|
||||
* @param[in] buf Buffer to send with SET_CUR, and receive with GET_CUR
|
||||
* @param[in] len Length of the buffer
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
static int
|
||||
uvc_xu_set_get_cur(libusb_device_handle *devh, int selector, uint8_t *buf, uint8_t len)
|
||||
{
|
||||
int ret = uvc_set_cur(devh, 0, ESP570_EXTENSION_UNIT, selector, buf, len);
|
||||
if (ret < 0) {
|
||||
U_LOG_E("uvc_set_cur failed: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return uvc_get_cur(devh, 0, ESP570_EXTENSION_UNIT, selector, buf, len);
|
||||
}
|
||||
|
||||
/*
|
||||
* Exported functions
|
||||
*/
|
||||
|
||||
int
|
||||
esp570_eeprom_read(libusb_device_handle *devh, uint16_t addr, uint8_t *data, uint8_t len)
|
||||
{
|
||||
uint8_t buf[59] = {0x82, 0xa0, addr >> 8, addr & 0xff, len};
|
||||
|
||||
if (len > 32) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_EEPROM, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
U_LOG_E("uvc_xu_set_get_cur failed: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (buf[0] != 0x82 || buf[1] != len) {
|
||||
U_LOG_E("read_buf: error: 0x%02x 0x%02x", buf[0], buf[1]);
|
||||
return -1;
|
||||
}
|
||||
|
||||
memcpy(data, buf + 2, len);
|
||||
return len;
|
||||
}
|
||||
|
||||
int
|
||||
esp570_i2c_read(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t *val)
|
||||
{
|
||||
uint8_t buf[6] = {0x84, addr, reg};
|
||||
|
||||
int ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_I2C, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (buf[0] != 0x84 || buf[4] != 0x00 || buf[5] != 0x00) {
|
||||
U_LOG_W("(%02x): %02x %02x %02x %02x %02x %02x", reg, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
|
||||
return -1;
|
||||
}
|
||||
|
||||
*val = (buf[1] << 8) | buf[2];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
esp570_i2c_write(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t val)
|
||||
{
|
||||
uint8_t buf[6] = {0x04, addr, reg, val >> 8, val & 0xff};
|
||||
|
||||
int ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_I2C, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (buf[0] != 0x04 || buf[1] != addr || buf[2] != reg || buf[5] != 0) {
|
||||
U_LOG_W("(%02x): %02x %02x %02x %02x %02x %02x", reg, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (buf[3] != (val >> 8) || buf[4] != (val & 0xff)) {
|
||||
U_LOG_W("(%02x, %04x): read back mismatch 0x%04x", reg, val, (buf[3] << 8) | buf[4]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
esp570_setup_unknown_3(libusb_device_handle *devh)
|
||||
{
|
||||
uint8_t buf[3];
|
||||
int ret;
|
||||
|
||||
buf[0] = 0x80;
|
||||
buf[1] = 0x14;
|
||||
buf[2] = 0x00;
|
||||
ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_UNKNOWN_3, buf, 3);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (buf[0] != 0x80 || buf[1] != 0xdc || buf[2] != 0x00) {
|
||||
U_LOG_W("(80 14 00): %02x %02x %02x", buf[0], buf[1], buf[2]);
|
||||
}
|
||||
|
||||
buf[0] = 0xa0;
|
||||
buf[1] = 0xf0;
|
||||
buf[2] = 0x00;
|
||||
ret = uvc_xu_set_get_cur(devh, ESP570_SELECTOR_UNKNOWN_3, buf, 3);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (buf[0] != 0xa0 || buf[1] != 0x98 || buf[2] != 0x00) {
|
||||
U_LOG_W("(80 14 00): %02x %02x %02x", buf[0], buf[1], buf[2]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
// Copyright 2014-2020, Philipp Zabel
|
||||
// Copyright 2017, TheOnlyJoey
|
||||
// Copyright 2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of esp570 sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author TheOnlyJoey <joeyferweda@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "xrt/xrt_defines.h"
|
||||
|
||||
#include <libusb.h>
|
||||
|
||||
|
||||
/*!
|
||||
* Performs a 16-bit read operation on the I2C bus.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] addr I2C address
|
||||
* @param[in] reg Register to read from
|
||||
* @param[out] val Value read
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
esp570_i2c_read(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t *val);
|
||||
|
||||
/*!
|
||||
* Performs a 16-bit write operation on the I2C bus.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] addr I2C address
|
||||
* @param[in] reg Register to write to
|
||||
* @param[in] val Value to write
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
esp570_i2c_write(libusb_device_handle *devh, uint8_t addr, uint8_t reg, uint16_t val);
|
||||
|
||||
/*!
|
||||
* Reads a buffer from the Microchip 24AA128 EEPROM.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] addr Address to read from
|
||||
* @param[out] buf Buffer to read into
|
||||
* @param[in] len Number of bytes to read (max 32)
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
esp570_eeprom_read(libusb_device_handle *devh, uint16_t addr, uint8_t *buf, uint8_t len);
|
||||
|
||||
/*!
|
||||
* Calls SET_CUR and GET_CUR on the extension unit's selector 3 with values
|
||||
* captured from the Oculus Windows drivers to setup the device. I have no idea what these mean.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
esp570_setup_unknown_3(libusb_device_handle *devh);
|
||||
@@ -0,0 +1,569 @@
|
||||
// Copyright 2017, Philipp Zabel
|
||||
// Copyright 2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of esp770u sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include "uvc/uvc_interface.h"
|
||||
|
||||
#include "esp770u.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#define ESP770U_EXTENSION_UNIT 4
|
||||
|
||||
#define ESP770U_SELECTOR_I2C 2
|
||||
#define ESP770U_SELECTOR_REG 3
|
||||
#define ESP770U_SELECTOR_COUNTER 10
|
||||
#define ESP770U_SELECTOR_CONTROL 11
|
||||
#define ESP770U_SELECTOR_DATA 12
|
||||
|
||||
#define XU_ENTITY ESP770U_EXTENSION_UNIT
|
||||
#define REG_SEL ESP770U_SELECTOR_REG
|
||||
#define CONTROL_SEL ESP770U_SELECTOR_CONTROL
|
||||
#define DATA_SEL ESP770U_SELECTOR_DATA
|
||||
|
||||
#define ESP_DEBUG 0
|
||||
|
||||
#define esp770u_read_reg(d, r, v) rift_sensor_esp770u_read_reg((d), 0xf0, (r), (v))
|
||||
#define esp770u_read_reg_f1(d, r, v) rift_sensor_esp770u_read_reg((d), 0xf1, (r), (v))
|
||||
#define esp770u_write_reg(d, r, v) rift_sensor_esp770u_write_reg((d), 0xf0, (r), (v))
|
||||
#define esp770u_write_reg_f1(d, r, v) rift_sensor_esp770u_write_reg((d), 0xf1, (r), (v))
|
||||
|
||||
/*
|
||||
* Internal functions
|
||||
*/
|
||||
|
||||
static int
|
||||
set_get_verify_a0(libusb_device_handle *dev, uint8_t val, uint8_t retval)
|
||||
{
|
||||
uint8_t buf[4] = {0xa0, val, 0x00, 0x00};
|
||||
int ret;
|
||||
|
||||
ret = uvc_set_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
memset(buf, 0, sizeof buf);
|
||||
ret = uvc_get_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (buf[0] != 0xa0 || buf[1] != retval || buf[2] != 0x00) {
|
||||
U_LOG_W("response, should be a0 %02x 00: %02x %02x %02x", retval, buf[0], buf[1], buf[2]);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Read self-incrementing counter.
|
||||
*/
|
||||
static int
|
||||
esp770u_get_counter(libusb_device_handle *devh, uint8_t *count)
|
||||
{
|
||||
return uvc_get_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_COUNTER, count, 1);
|
||||
}
|
||||
|
||||
/*
|
||||
* Write back self-incrementing counter.
|
||||
*/
|
||||
static int
|
||||
esp770u_set_counter(libusb_device_handle *devh, uint8_t count)
|
||||
{
|
||||
return uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_COUNTER, &count, 1);
|
||||
}
|
||||
|
||||
static int
|
||||
esp770u_spi_set_control(libusb_device_handle *devh, uint8_t a, size_t len)
|
||||
{
|
||||
// a is alternating, 0x81 or 0x41
|
||||
uint8_t control[16] = {0x00, a, 0x80, 0x01, [9] = len};
|
||||
|
||||
return uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_CONTROL, control, sizeof control);
|
||||
}
|
||||
|
||||
static int
|
||||
esp770u_spi_set_data(libusb_device_handle *devh, uint8_t *data, size_t len)
|
||||
{
|
||||
return uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_DATA, data, len);
|
||||
}
|
||||
|
||||
static int
|
||||
esp770u_spi_get_data(libusb_device_handle *devh, uint8_t *data, size_t len)
|
||||
{
|
||||
return uvc_get_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_DATA, data, len);
|
||||
}
|
||||
|
||||
static int
|
||||
radio_write(libusb_device_handle *devhandle, const uint8_t *buf, size_t len)
|
||||
{
|
||||
uint8_t data[127];
|
||||
int ret;
|
||||
size_t i;
|
||||
|
||||
if (len > 126) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
memset(data, 0, sizeof data);
|
||||
for (i = 0; i < len; i++) {
|
||||
data[i] = buf[i];
|
||||
data[126] -= buf[i]; // calculate checksum
|
||||
}
|
||||
|
||||
ret = esp770u_spi_set_control(devhandle, 0x81, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// send data
|
||||
ret = esp770u_spi_set_data(devhandle, data, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = esp770u_spi_set_control(devhandle, 0x41, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// expect all zeros
|
||||
ret = esp770u_spi_get_data(devhandle, data, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = esp770u_spi_set_control(devhandle, 0x81, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// clear
|
||||
memset(data, 0, sizeof data);
|
||||
ret = esp770u_spi_set_data(devhandle, data, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = esp770u_spi_set_control(devhandle, 0x41, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = esp770u_spi_get_data(devhandle, data, sizeof data);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Expect zeros
|
||||
for (i = 2; i < 126; i++) {
|
||||
if (data[i]) {
|
||||
U_LOG_W("Unexpected byte (%02x at %02x):", data[i], (uint8_t)i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (data[0] != buf[0] || data[1] != buf[1]) {
|
||||
U_LOG_W("Unexpected read (%02x %02x):", buf[0], buf[1]);
|
||||
for (i = 0; i < 127; i++) {
|
||||
U_LOG_D("%02x ", data[i]);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t checksum = 0;
|
||||
for (i = 0; i < 127; i++) {
|
||||
checksum += data[i];
|
||||
}
|
||||
if (checksum) {
|
||||
U_LOG_W("Checksum mismatch: %02x", checksum);
|
||||
for (i = 0; i < 127; i++) {
|
||||
U_LOG_D("%02x ", data[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
rift_sensor_esp770u_read_reg(libusb_device_handle *dev, uint8_t reg_block, uint8_t reg, uint8_t *val)
|
||||
{
|
||||
uint8_t buf[4] = {0x82, reg_block, reg};
|
||||
int ret;
|
||||
|
||||
ret = uvc_set_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = uvc_get_cur(dev, 0, XU_ENTITY, REG_SEL, buf, 3);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (buf[0] != 0x82 || buf[2] != 0x00) {
|
||||
U_LOG_W("read_reg(0x%x,0x%x): %02x %02x %02x", reg_block, reg, buf[0], buf[1], buf[2]);
|
||||
}
|
||||
*val = buf[1];
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int
|
||||
rift_sensor_esp770u_write_reg(libusb_device_handle *dev, uint8_t reg_block, uint8_t reg, uint8_t val)
|
||||
{
|
||||
uint8_t buf[4] = {0x02, reg_block, reg, val};
|
||||
int ret;
|
||||
|
||||
ret = uvc_set_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = uvc_get_cur(dev, 0, XU_ENTITY, REG_SEL, buf, sizeof buf);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (buf[0] != 0x02 || buf[1] != reg_block || buf[2] != reg || buf[3] != val) {
|
||||
U_LOG_W("write_reg(0x%x,0x%x,0x%x): %02x %02x %02x %02x", reg_block, reg, val, buf[0], buf[1], buf[2],
|
||||
buf[3]);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
* Exported functions
|
||||
*/
|
||||
|
||||
int
|
||||
rift_sensor_esp770u_flash_read(libusb_device_handle *devh, uint32_t addr, uint8_t *data, uint16_t len)
|
||||
{
|
||||
uint8_t control[16];
|
||||
uint8_t count;
|
||||
int ret;
|
||||
|
||||
ret = esp770u_get_counter(devh, &count);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
memset(control, 0, sizeof control);
|
||||
control[0] = count;
|
||||
control[1] = 0x41;
|
||||
control[2] = 0x03;
|
||||
control[3] = 0x01;
|
||||
|
||||
control[5] = (addr >> 16) & 0xff;
|
||||
control[6] = (addr >> 8) & 0xff;
|
||||
control[7] = addr & 0xff;
|
||||
|
||||
control[8] = len >> 8;
|
||||
control[9] = len & 0xff;
|
||||
|
||||
ret = uvc_set_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_CONTROL, control, sizeof control);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
memset(data, 0, len);
|
||||
ret = uvc_get_cur(devh, 0, ESP770U_EXTENSION_UNIT, ESP770U_SELECTOR_DATA, data, len);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return esp770u_set_counter(devh, count);
|
||||
}
|
||||
|
||||
int
|
||||
rift_sensor_esp770u_setup_radio(libusb_device_handle *devhandle, const uint8_t radio_id[5])
|
||||
{
|
||||
int ret;
|
||||
|
||||
const uint8_t cmd1[7] = {0x40, 0x10, radio_id[0], radio_id[1], radio_id[2], radio_id[3], radio_id[4]};
|
||||
ret = radio_write(devhandle, cmd1, sizeof cmd1);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
const uint8_t cmd2[10] = {0x50, 0x11, 0xf4, 0x01, 0x00, 0x00, 0x67, 0xff, 0xff, 0xff};
|
||||
ret = radio_write(devhandle, cmd2, sizeof cmd2);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
const uint8_t cmd3[2] = {0x61, 0x12};
|
||||
ret = radio_write(devhandle, cmd3, sizeof cmd3);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
const uint8_t cmd4[2] = {0x71, 0x85};
|
||||
ret = radio_write(devhandle, cmd4, sizeof cmd4);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
const uint8_t cmd5[2] = {0x81, 0x86};
|
||||
return radio_write(devhandle, cmd5, sizeof cmd5);
|
||||
}
|
||||
|
||||
int
|
||||
rift_sensor_esp770u_init_regs(libusb_device_handle *devhandle)
|
||||
{
|
||||
int ret;
|
||||
uint8_t val;
|
||||
|
||||
ret = set_get_verify_a0(devhandle, 0x03, 0xb2);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = esp770u_read_reg(devhandle, 0x5a, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x01 && val != 0x03) {
|
||||
U_LOG_W("unexpected 5a value: %02x", val);
|
||||
}
|
||||
|
||||
ret = esp770u_write_reg(devhandle, 0x5a, 0x01); /* &= 0x02? */
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_read_reg(devhandle, 0x5a, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
if (val != 0x01) {
|
||||
U_LOG_W("unexpected 5a value: %02x", val);
|
||||
}
|
||||
|
||||
ret = esp770u_read_reg(devhandle, 0x18, &val); /* |= 0x01 ? */
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg(devhandle, 0x18, 0x0f);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_read_reg(devhandle, 0x17, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg(devhandle, 0x17, 0xed); /* |= 0x01 ? */
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg(devhandle, 0x17, 0xec); /* &= ~0x01 ? */
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg(devhandle, 0x18, 0x0e); /* &= ~0x01 ? */
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_read_reg(devhandle, 0x14, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
#if ESP_DEBUG
|
||||
for (int i = 0; i < 256; i += 1) {
|
||||
uint8_t rval;
|
||||
if (i % 16 == 0) {
|
||||
printf("%02x: ", i);
|
||||
}
|
||||
ret = esp770u_read_reg(devhandle, i, &rval);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
printf("%02x ", rval);
|
||||
if (i % 16 == 15) {
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
printf("--------------------------------------\n");
|
||||
|
||||
for (int i = 0; i < 256; i += 1) {
|
||||
uint8_t rval;
|
||||
if (i % 16 == 0) {
|
||||
printf("%02x: ", i);
|
||||
}
|
||||
ret = esp770u_read_reg_f1(devhandle, i, &rval);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
printf("%02x ", rval);
|
||||
if (i % 16 == 15) {
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
printf("--------------------------------------\n");
|
||||
#endif
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
rift_sensor_esp770u_init_jpeg(libusb_device_handle *devhandle)
|
||||
{
|
||||
int ret = 0;
|
||||
uint8_t val;
|
||||
|
||||
/* 0xf11f = 0x04 */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x1f, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x1f, 0x04);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf102 = 0x7f */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x02, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf101 = 0xff */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x01, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf103 = 0xc0 */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x03, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x03, 0xc0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf101 = 0xff, 2 times */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x01, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x01, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf11e = 0x01 */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x1e, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x1e, 0x01);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf103 = 0xc0 */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x03, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x03, 0xc0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf101 = 0xff, once */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x01, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf102 = 0x7f again */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x02, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 0xf101 = 0xff, 2 more times */
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x01, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x01, 0xff);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// @todo is this early return intentional?
|
||||
return ret;
|
||||
|
||||
|
||||
// https://discord.com/channels/556527313823596604/1140622730018967623/1471716745164230807
|
||||
#if 0
|
||||
// 0xf102 = 0x7f, 2 more times
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x02, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = esp770u_read_reg_f1(devhandle, 0x02, &val);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
ret = esp770u_write_reg_f1(devhandle, 0x02, 0x7f);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Extra 64-byte fw writes, not sent for now - doesn't seem to matter:
|
||||
* Data Fragment: 1681030100012ed10040000000000000
|
||||
* Data Fragment: 070707070707070707070707070707070707070707070707070707070707070707070707…
|
||||
* Data Fragment: 16020100000000000000000000000000
|
||||
* Data Fragment: 16
|
||||
* Data Fragment: 1781030100012f160040000000000000
|
||||
* Data Fragment: 010101010101010101010101010101010101010101010101010101010101010101010101…
|
||||
* Data Fragment: 17020100000000000000000000000000
|
||||
* Data Fragment: 17
|
||||
*/
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright 2017, Philipp Zabel
|
||||
// Copyright 2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of esp770u sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "xrt/xrt_defines.h"
|
||||
|
||||
#include <libusb.h>
|
||||
|
||||
|
||||
/*!
|
||||
* Reads a buffer from the flash storage.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] addr Address to read from
|
||||
* @param[out] data Buffer to read into
|
||||
* @param[in] len Number of bytes to read
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
rift_sensor_esp770u_flash_read(libusb_device_handle *devh, uint32_t addr, uint8_t *data, uint16_t len);
|
||||
|
||||
/*!
|
||||
* Sets up the radio with the given ID.
|
||||
*
|
||||
* @param[in] devhandle libusb device handle
|
||||
* @param[in] radio_id 5-byte radio ID
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
rift_sensor_esp770u_setup_radio(libusb_device_handle *devhandle, const uint8_t radio_id[5]);
|
||||
|
||||
/*!
|
||||
* Initial register setup, only after camera plugin
|
||||
*
|
||||
* @param[in] devhandle libusb device handle
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
rift_sensor_esp770u_init_regs(libusb_device_handle *devhandle);
|
||||
|
||||
/*!
|
||||
* Extra initialisation sent after UVC config when in USB2 / MJPEG mode
|
||||
*
|
||||
* @param[in] devhandle libusb device handle
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
rift_sensor_esp770u_init_jpeg(libusb_device_handle *devhandle);
|
||||
@@ -0,0 +1,164 @@
|
||||
// Copyright 2014-2020, Philipp Zabel
|
||||
// Copyright 2017, TheOnlyJoey
|
||||
// Copyright 2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of mt9v034 sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author TheOnlyJoey <joeyferweda@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include "esp570.h"
|
||||
#include "mt9v034.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#define MT9V034_CHIP_VERSION 0x00
|
||||
#define MT9V034_WINDOW_HEIGHT 0x03
|
||||
#define MT9V034_WINDOW_WIDTH 0x04
|
||||
#define MT9V034_HORIZONTAL_BLANKING 0x05
|
||||
#define MT9V034_VERTICAL_BLANKING 0x06
|
||||
#define MT9V034_CHIP_CONTROL 0x07
|
||||
#define MT9V034_COARSE_SHUTTER_WIDTH_TOTAL 0x0b
|
||||
#define MT9V034_READ_MODE 0x0d
|
||||
#define MT9V034_LED_OUT_CONTROL 0x1b
|
||||
#define MT9V034_ANALOG_GAIN 0x35
|
||||
#define MT9V034_BLACK_LEVEL_CALIB_CTRL 0x47
|
||||
#define MT9V034_BLACK_LEVEL_CALIB_VALUE 0x48
|
||||
#define MT9V034_AEC_AGC_ENABLE 0xaf
|
||||
#define MT9V034_MAX_TOTAL_SHUTTER_WIDTH 0xbd
|
||||
#define MT9V034_FINE_SHUTTER_WIDTH_TOTAL 0xd5
|
||||
|
||||
#define MT9V034_ANALOG_GAIN_MIN 16
|
||||
|
||||
#define MT9V034_CHIP_CONTROL_MASTER_MODE (1 << 3)
|
||||
#define MT9V034_CHIP_CONTROL_SNAPSHOT_MODE (3 << 3)
|
||||
#define MT9V034_CHIP_CONTROL_DOUT_ENABLE (1 << 7)
|
||||
#define MT9V034_CHIP_CONTROL_SEQUENTIAL (1 << 8)
|
||||
|
||||
#define MT9V032_READ_MODE_RESERVED 0x300
|
||||
#define MT9V034_READ_MODE_ROW_FLIP (1 << 4)
|
||||
#define MT9V034_READ_MODE_COLUMN_FLIP (1 << 5)
|
||||
|
||||
#define MT9V034_BLACK_LEVEL_CALIB_OVERRIDE (1 << 0)
|
||||
|
||||
#define MT9V034_LED_OUT_DISABLE (1 << 0)
|
||||
#define MT9V034_LED_OUT_INVERT (1 << 1)
|
||||
|
||||
#define MT9V034_I2C_ADDR 0x4c
|
||||
|
||||
/*
|
||||
* Internal functions
|
||||
*/
|
||||
|
||||
static inline int
|
||||
mt9v034_read_reg(libusb_device_handle *devh, uint8_t reg, uint16_t *val)
|
||||
{
|
||||
return esp570_i2c_read(devh, MT9V034_I2C_ADDR << 1, reg, val);
|
||||
}
|
||||
|
||||
static inline int
|
||||
mt9v034_write_reg(libusb_device_handle *devh, uint8_t reg, uint16_t val)
|
||||
{
|
||||
return esp570_i2c_write(devh, MT9V034_I2C_ADDR << 1, reg, val);
|
||||
}
|
||||
|
||||
static int
|
||||
mt9v034_enable_sync(libusb_device_handle *devh)
|
||||
{
|
||||
uint16_t chip_control;
|
||||
|
||||
// Disable AEC/AGC
|
||||
mt9v034_write_reg(devh, MT9V034_AEC_AGC_ENABLE, 0);
|
||||
// Raise black level with manual black level calibration override
|
||||
mt9v034_write_reg(devh, MT9V034_BLACK_LEVEL_CALIB_CTRL, MT9V034_BLACK_LEVEL_CALIB_OVERRIDE);
|
||||
mt9v034_write_reg(devh, MT9V034_BLACK_LEVEL_CALIB_VALUE, 0x81); // -127
|
||||
// Reduce analog gain to minimum
|
||||
mt9v034_write_reg(devh, MT9V034_ANALOG_GAIN, MT9V034_ANALOG_GAIN_MIN);
|
||||
mt9v034_write_reg(devh, MT9V034_VERTICAL_BLANKING, 5);
|
||||
mt9v034_write_reg(devh, MT9V034_HORIZONTAL_BLANKING, 94);
|
||||
mt9v034_write_reg(devh, MT9V034_CHIP_CONTROL,
|
||||
MT9V034_CHIP_CONTROL_MASTER_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE |
|
||||
MT9V034_CHIP_CONTROL_SEQUENTIAL);
|
||||
|
||||
// Set integration time in number of rows + number of clock cycles
|
||||
mt9v034_write_reg(devh, MT9V034_COARSE_SHUTTER_WIDTH_TOTAL, 11);
|
||||
mt9v034_write_reg(devh, MT9V034_FINE_SHUTTER_WIDTH_TOTAL, 111);
|
||||
// Switch to snapshot mode, exposure controlled by Rift DK2 HMD
|
||||
mt9v034_read_reg(devh, MT9V034_CHIP_CONTROL, &chip_control);
|
||||
if (chip_control !=
|
||||
(MT9V034_CHIP_CONTROL_MASTER_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE | MT9V034_CHIP_CONTROL_SEQUENTIAL)) {
|
||||
U_LOG_W("Unexpected chip control value: 0x%04x", chip_control);
|
||||
}
|
||||
mt9v034_write_reg(devh, MT9V034_CHIP_CONTROL,
|
||||
MT9V034_CHIP_CONTROL_SNAPSHOT_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE |
|
||||
MT9V034_CHIP_CONTROL_SEQUENTIAL);
|
||||
// Enable LED?
|
||||
mt9v034_write_reg(devh, MT9V034_LED_OUT_CONTROL, MT9V034_LED_OUT_INVERT);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
mt9v034_disable_sync(libusb_device_handle *devh)
|
||||
{
|
||||
// Disable LED?
|
||||
mt9v034_write_reg(devh, MT9V034_LED_OUT_CONTROL, 0);
|
||||
|
||||
// Disable AEC/AGC
|
||||
mt9v034_write_reg(devh, MT9V034_AEC_AGC_ENABLE, 0);
|
||||
// Raise black level with manual black level calibration override
|
||||
mt9v034_write_reg(devh, MT9V034_BLACK_LEVEL_CALIB_CTRL, 0x80);
|
||||
// Set analog gain to default
|
||||
mt9v034_write_reg(devh, MT9V034_ANALOG_GAIN, 0x20);
|
||||
mt9v034_write_reg(devh, MT9V034_VERTICAL_BLANKING, 57);
|
||||
mt9v034_write_reg(devh, MT9V034_HORIZONTAL_BLANKING, 94);
|
||||
mt9v034_write_reg(devh, MT9V034_CHIP_CONTROL,
|
||||
MT9V034_CHIP_CONTROL_MASTER_MODE | MT9V034_CHIP_CONTROL_DOUT_ENABLE |
|
||||
MT9V034_CHIP_CONTROL_SEQUENTIAL);
|
||||
// Set integration time in number of rows + number of clock cycles
|
||||
mt9v034_write_reg(devh, MT9V034_COARSE_SHUTTER_WIDTH_TOTAL, 0xf0);
|
||||
mt9v034_write_reg(devh, MT9V034_FINE_SHUTTER_WIDTH_TOTAL, 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Exported functions
|
||||
*/
|
||||
|
||||
int
|
||||
mt9v034_setup(libusb_device_handle *devh)
|
||||
{
|
||||
uint16_t version, read_mode;
|
||||
|
||||
mt9v034_read_reg(devh, MT9V034_CHIP_VERSION, &version);
|
||||
if (version != 0x1324) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Enable horizontal and vertical flip
|
||||
mt9v034_read_reg(devh, MT9V034_READ_MODE, &read_mode);
|
||||
mt9v034_write_reg(devh, MT9V034_READ_MODE,
|
||||
read_mode | MT9V034_READ_MODE_ROW_FLIP | MT9V034_READ_MODE_COLUMN_FLIP);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
mt9v034_set_sync(libusb_device_handle *devh, bool enabled)
|
||||
{
|
||||
if (enabled) {
|
||||
return mt9v034_enable_sync(devh);
|
||||
} else {
|
||||
return mt9v034_disable_sync(devh);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
// Copyright 2014-2020, Philipp Zabel
|
||||
// Copyright 2017, TheOnlyJoey
|
||||
// Copyright 2025, Jan Schmidt
|
||||
// Copyright 2025-2026, Beyley Cardellio
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Implementation of mt9v034 sensor initialization
|
||||
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
||||
* @author TheOnlyJoey <joeyferweda@gmail.com>
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
|
||||
* @ingroup drv_rift_sensor
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "xrt/xrt_defines.h"
|
||||
|
||||
#include <libusb.h>
|
||||
|
||||
|
||||
/*!
|
||||
* Sets up the MT9V034 sensor for synchronized exposure, with minimal gain
|
||||
* and raised black level calibration.
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
mt9v034_setup(libusb_device_handle *devh);
|
||||
|
||||
/*!
|
||||
* Enables or disables the MT9V034's sync output
|
||||
*
|
||||
* @param[in] devh libusb device handle
|
||||
* @param[in] enabled Whether to enable or disable the sync output
|
||||
* @return 0 on success, negative error code on failure
|
||||
*/
|
||||
int
|
||||
mt9v034_set_sync(libusb_device_handle *devh, bool enabled);
|
||||
Reference in new issue
Block a user