From 3ef82585695605d40be9bd0a1dc4a882fbb3a4af Mon Sep 17 00:00:00 2001 From: Yannis Gravezas Date: Tue, 23 Apr 2013 14:35:33 +0300 Subject: [PATCH] Kinect for Windows and Xbox 360 Model 1473 support This patch does auto detection of all three models and disables motor subdevice for the 2 newer ones. It also adds a function to test for the presence of the motor and audio devices on newer models. Signed-off-by: Yannis Gravezas (wizgrav) --- include/libfreenect.h | 9 ++++ .../windows/libusb10emu/libusb-1.0/libusb.h | 1 + .../libusb10emu/libusb-1.0/libusbemu.cpp | 11 +++++ src/cameras.c | 6 +-- src/core.c | 4 ++ src/freenect_internal.h | 3 ++ src/tilt.c | 8 ++++ src/usb_libusb10.c | 42 ++++++++++++++++--- 8 files changed, 75 insertions(+), 9 deletions(-) diff --git a/include/libfreenect.h b/include/libfreenect.h index 13c7c4c5..1648a756 100644 --- a/include/libfreenect.h +++ b/include/libfreenect.h @@ -302,6 +302,15 @@ FREENECTAPI int freenect_supported_subdevices(void); */ FREENECTAPI void freenect_select_subdevices(freenect_context *ctx, freenect_device_flags subdevs); +/** + * Returns the devices that are enabled after calls to freenect_open_device() + * On newer kinects the motor and audio are automatically disabled for now + * + * @param ctx Context to set future subdevice selection for + * @return Flags representing the subdevices that were actually opened (see freenect_device_flags) + */ +FREENECTAPI freenect_device_flags freenect_enabled_subdevices(freenect_context *ctx); + /** * Opens a kinect device via a context. Index specifies the index of * the device on the current state of the bus. Bus resets may cause diff --git a/platform/windows/libusb10emu/libusb-1.0/libusb.h b/platform/windows/libusb10emu/libusb-1.0/libusb.h index 33b32887..38c3c1a9 100644 --- a/platform/windows/libusb10emu/libusb-1.0/libusb.h +++ b/platform/windows/libusb10emu/libusb-1.0/libusb.h @@ -77,6 +77,7 @@ int libusb_get_string_descriptor(libusb_device_handle *dev_handle, uint8_t desc_ int libusb_get_string_descriptor_ascii(libusb_device_handle *dev_handle, uint8_t desc_index, unsigned char *data, int length); int libusb_set_configuration(libusb_device_handle *dev, int configuration); +int libusb_set_interface_alt_setting(libusb_device_handle *dev,int interface_number,int alternate_setting); int libusb_claim_interface(libusb_device_handle* dev, int interface_number); int libusb_release_interface(libusb_device_handle* dev, int interface_number); diff --git a/platform/windows/libusb10emu/libusb-1.0/libusbemu.cpp b/platform/windows/libusb10emu/libusb-1.0/libusbemu.cpp index af39ff68..79361e1f 100644 --- a/platform/windows/libusb10emu/libusb-1.0/libusbemu.cpp +++ b/platform/windows/libusb10emu/libusb-1.0/libusbemu.cpp @@ -302,6 +302,17 @@ int libusb_set_configuration(libusb_device_handle *dev, int configuration) return 0; } +int libusb_set_interface_alt_setting(libusb_device_handle *dev, int interface_number,int alternate_setting){ + RAIIMutex lock (dev->dev->ctx->mutex); + if (0 != usb_set_altinterface(dev->handle, alternate_setting)) + { + LIBUSBEMU_ERROR_LIBUSBWIN32(); + return(LIBUSB_ERROR_OTHER); + } + + return(0); +} + int libusb_claim_interface(libusb_device_handle* dev, int interface_number) { RAIIMutex lock (dev->dev->ctx->mutex); diff --git a/src/cameras.c b/src/cameras.c index 7886a332..0483c2ed 100644 --- a/src/cameras.c +++ b/src/cameras.c @@ -904,9 +904,9 @@ static int freenect_fetch_zero_plane_info(freenect_device *dev) uint16_t cmd[5] = {0}; // Offset is the only field in this command, and it's 0 int res; - res = send_cmd(dev, 0x04, cmd, 10, reply, 322); //OPCODE_GET_FIXED_PARAMS = 4, - if (res != 322) { - FN_ERROR("freenect_fetch_zero_plane_info: send_cmd read %d bytes (expected 322)\n", res); + res = send_cmd(dev, 0x04, cmd, 10, reply, ctx->zero_plane_res); //OPCODE_GET_FIXED_PARAMS = 4, + if (res != ctx->zero_plane_res) { + FN_ERROR("freenect_fetch_zero_plane_info: send_cmd read %d bytes (expected %d)\n", res,ctx->zero_plane_res); return -1; } diff --git a/src/core.c b/src/core.c index ecc5942f..305c9e18 100644 --- a/src/core.c +++ b/src/core.c @@ -148,6 +148,10 @@ FREENECTAPI void freenect_select_subdevices(freenect_context *ctx, freenect_devi )); } +FREENECTAPI freenect_device_flags freenect_enabled_subdevices(freenect_context *ctx) { + return ctx->enabled_subdevices; +} + FREENECTAPI int freenect_open_device(freenect_context *ctx, freenect_device **dev, int index) { int res; diff --git a/src/freenect_internal.h b/src/freenect_internal.h index 640d23b6..8df4ee22 100644 --- a/src/freenect_internal.h +++ b/src/freenect_internal.h @@ -52,6 +52,7 @@ struct _freenect_context { fnusb_ctx usb; freenect_device_flags enabled_subdevices; freenect_device *first; + int zero_plane_res; }; #define LL_FATAL FREENECT_LOG_FATAL @@ -131,6 +132,8 @@ static inline int32_t fn_le32s(int32_t s) #define PID_NUI_AUDIO 0x02ad #define PID_NUI_CAMERA 0x02ae #define PID_NUI_MOTOR 0x02b0 +#define PID_K4W_CAMERA 0x02bf +#define PID_K4W_AUDIO 0x02be typedef struct { int running; diff --git a/src/tilt.c b/src/tilt.c index 1a466499..8852e768 100644 --- a/src/tilt.c +++ b/src/tilt.c @@ -47,6 +47,8 @@ freenect_raw_tilt_state* freenect_get_tilt_state(freenect_device *dev) int freenect_update_tilt_state(freenect_device *dev) { freenect_context *ctx = dev->parent; + if(!(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)) + return 0; uint8_t buf[10]; uint16_t ux, uy, uz; int ret = fnusb_control(&dev->usb_motor, 0xC0, 0x32, 0x0, 0x0, buf, 10); @@ -70,6 +72,9 @@ int freenect_update_tilt_state(freenect_device *dev) int freenect_set_tilt_degs(freenect_device *dev, double angle) { + freenect_context *ctx = dev->parent; + if(!(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)) + return 0; int ret; uint8_t empty[0x1]; @@ -82,6 +87,9 @@ int freenect_set_tilt_degs(freenect_device *dev, double angle) int freenect_set_led(freenect_device *dev, freenect_led_options option) { + freenect_context *ctx = dev->parent; + if(!(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)) + return 0; int ret; uint8_t empty[0x1]; ret = fnusb_control(&dev->usb_motor, 0x40, 0x06, (uint16_t)option, 0x0, empty, 0x0); diff --git a/src/usb_libusb10.c b/src/usb_libusb10.c index 065bf6f6..451d2012 100644 --- a/src/usb_libusb10.c +++ b/src/usb_libusb10.c @@ -47,7 +47,7 @@ FN_INTERNAL int fnusb_num_devices(fnusb_ctx *ctx) int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; - if (desc.idVendor == VID_MICROSOFT && desc.idProduct == PID_NUI_CAMERA) + if (desc.idVendor == VID_MICROSOFT && (desc.idProduct == PID_NUI_CAMERA || desc.idProduct == PID_K4W_CAMERA)) nr++; } libusb_free_device_list (devs, 1); @@ -76,7 +76,7 @@ FN_INTERNAL int fnusb_list_device_attributes(fnusb_ctx *ctx, struct freenect_dev int r = libusb_get_device_descriptor (devs[i], &desc); if (r < 0) continue; - if (desc.idVendor == VID_MICROSOFT && desc.idProduct == PID_NUI_CAMERA) { + if (desc.idVendor == VID_MICROSOFT && (desc.idProduct == PID_NUI_CAMERA || desc.idProduct == PID_K4W_CAMERA)) { // Verify that a serial number exists to query. If not, don't touch the device. if (desc.iSerialNumber == 0) { continue; @@ -188,9 +188,9 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) if (desc.idVendor != VID_MICROSOFT) continue; - + res = 0; // Search for the camera - if ((ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA) && !dev->usb_cam.dev && desc.idProduct == PID_NUI_CAMERA) { + if ((ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA) && !dev->usb_cam.dev && (desc.idProduct == PID_NUI_CAMERA || desc.idProduct == PID_K4W_CAMERA)) { // If the index given by the user matches our camera index if (nr_cam == index) { res = libusb_open (devs[i], &dev->usb_cam.dev); @@ -199,6 +199,15 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) dev->usb_cam.dev = NULL; break; } + if(desc.idProduct == PID_K4W_CAMERA || desc.bcdDevice != fn_le32(267)){ + /* Not the old kinect so we only set up the camera*/ + ctx->enabled_subdevices = FREENECT_DEVICE_CAMERA; + ctx->zero_plane_res = 334; + }else{ + /* The good old kinect that tilts and tweets */ + ctx->zero_plane_res = 322; + } + #ifndef _WIN32 // Detach an existing kernel driver for the device res = libusb_kernel_driver_active(dev->usb_cam.dev, 0); @@ -219,12 +228,33 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) dev->usb_cam.dev = NULL; break; } + if(desc.idProduct == PID_K4W_CAMERA){ + res = libusb_set_interface_alt_setting(dev->usb_cam.dev, 0, 1); + if (res != 0) { + FN_ERROR("Failed to set alternate interface #1 for K4W: %d\n", res); + libusb_close(dev->usb_cam.dev); + dev->usb_cam.dev = NULL; + break; + } + + } } else { nr_cam++; } } - + } + + if(ctx->enabled_subdevices == FREENECT_DEVICE_CAMERA || res < 0) cnt = 0; + // Search for the motor + + for (i = 0; i < cnt; i++) { + int r = libusb_get_device_descriptor (devs[i], &desc); + if (r < 0) + continue; + + if (desc.idVendor != VID_MICROSOFT) + continue; if ((ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR) && !dev->usb_motor.dev && desc.idProduct == PID_NUI_MOTOR) { // If the index given by the user matches our camera index if (nr_mot == index) { @@ -249,7 +279,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) #ifdef BUILD_AUDIO // TODO: check that the firmware has already been loaded; if not, upload firmware. // Search for the audio - if ((ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO) && !dev->usb_audio.dev && desc.idProduct == PID_NUI_AUDIO) { + if ((ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO) && !dev->usb_audio.dev && (desc.idProduct == PID_NUI_AUDIO || desc.idProduct == PID_K4W_AUDIO)) { // If the index given by the user matches our audio index if (nr_audio == index) { res = libusb_open (devs[i], &dev->usb_audio.dev);