Add support for near mode (K4W only)

Signed-off-by: Benn Snyder <[email protected]>

Author: piedar

CMakeLists.txt

@@ -48,7 +48,7 @@ include (SetupDirectories)
 
 set (PROJECT_VER_MAJOR 0)
 set (PROJECT_VER_MINOR 5)
-set (PROJECT_VER_PATCH 0)
+set (PROJECT_VER_PATCH 1)
 set (PROJECT_VER
   "${PROJECT_VER_MAJOR}.${PROJECT_VER_MINOR}.${PROJECT_VER_PATCH}")
 set (PROJECT_APIVER

examples/glview.c

@@ -231,6 +231,11 @@ void keyPressed(unsigned char key, int x, int y)
 		freenect_set_flag(f_dev, FREENECT_MIRROR_VIDEO, mirror);
 		mirror = mirror ? FREENECT_OFF : FREENECT_ON;
 	}
+	if (key == 'n') {
+		static freenect_flag_value near = FREENECT_ON;
+		freenect_set_flag(f_dev, FREENECT_NEAR_MODE, near);
+		near = near ? FREENECT_OFF : FREENECT_ON;
+	}
 	if (key == '1') {
 		freenect_set_led(f_dev,LED_GREEN);
 	}
@@ -416,7 +421,7 @@ void *freenect_threadfunc(void *arg)
 
 	printf("'w' - tilt up, 's' - level, 'x' - tilt down, '0'-'6' - select LED mode \n");
 	printf("'f' - change video format, 'm' - mirror video, 'o' - rotate video with accelerometer \n");
-	printf("'e' - auto exposure, 'b' - white balance, 'r' - raw color \n");
+	printf("'e' - auto exposure, 'b' - white balance, 'r' - raw color, 'n' - near mode (K4W only) \n");
 
 	while (!die && freenect_process_events(f_ctx) >= 0) {
 		//Throttle the text output

include/libfreenect.h

@@ -115,6 +115,7 @@ typedef enum {
 	// arbitrary bitfields to support flag combination
 	FREENECT_MIRROR_DEPTH       = 1 << 16,
 	FREENECT_MIRROR_VIDEO       = 1 << 17,
+	FREENECT_NEAR_MODE          = 1 << 18, // K4W only
 } freenect_flag;
 
 /// Possible values for setting each `freenect_flag`

platform/windows/unistd.h

@@ -27,6 +27,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <windows.h>
 
 // MinGW defines _SSIZE_T_DEFINED in sys/types.h when it defines ssize_t to be a long.
 // Redefining it causes an error.
@@ -35,3 +36,16 @@
 #define _SSIZE_T_DEFINED
 typedef long ssize_t;
 #endif // _SSIZE_T_DEFINED
+
+
+void usleep(__int64 usec)
+{
+	// Convert to 100 nanosecond interval, negative for relative time.
+	LARGE_INTEGER ft;
+	ft.QuadPart = -(10 * usec);
+
+	HANDLE timer = CreateWaitableTimer(NULL, TRUE, NULL);
+	SetWaitableTimer(timer, &ft, 0, NULL, NULL, 0);
+	WaitForSingleObject(timer, INFINITE);
+	CloseHandle(timer);
+}

src/core.c

@@ -188,7 +188,7 @@ FREENECTAPI int freenect_open_device_by_camera_serial(freenect_context *ctx, fre
 	int count = fnusb_list_device_attributes(&ctx->usb, &attrlist);
 	if (count < 0) {
 		FN_ERROR("freenect_open_device_by_camera_serial: Couldn't enumerate serial numbers\n");
-		return -1;
+		return count;
 	}
 	int index = 0;
 	for(item = attrlist ; item != NULL; item = item->next , index++) {

src/flags.c

@@ -25,6 +25,7 @@
  */
 
 #include <string.h> // for memcpy
+#include <unistd.h> // for usleep
 #include "freenect_internal.h"
 #include "flags.h"
 
@@ -47,6 +48,34 @@ FN_INTERNAL int register_for_flag(int flag)
 
 int freenect_set_flag(freenect_device *dev, freenect_flag flag, freenect_flag_value value)
 {
+	freenect_context *ctx = dev->parent;
+
+	if (flag == FREENECT_NEAR_MODE)
+	{
+		if (dev->usb_cam.PID != PID_K4W_CAMERA)
+		{
+			FN_WARNING("Near mode is only supported by K4W");
+			return -1;
+		}
+
+		if (value == FREENECT_ON)
+		{
+			int ret = write_register(dev, 0x0015, 0x0007);
+			if (ret < 0)
+				return ret;
+			usleep(100000);
+			return write_register(dev, 0x02EF, 0x0000);
+		}
+		else
+		{
+			int ret = write_register(dev, 0x0015, 0x001E);
+			if (ret < 0)
+				return ret;
+			usleep(100000);
+			return write_register(dev, 0x02EF, 0x0190);
+		}
+	}
+
     if (flag >= (1 << 16))
     {
         int reg = register_for_flag(flag);

src/freenect_internal.h

@@ -253,6 +253,6 @@ struct _freenect_device {
 	fnusb_dev usb_motor;
 	freenect_raw_tilt_state raw_state;
 
-    int device_does_motor_control_with_audio;
-    int motor_control_with_audio_enabled;
+	int device_does_motor_control_with_audio;
+	int motor_control_with_audio_enabled;
 };

src/usb_libusb10.c

@@ -291,6 +291,9 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
 			// If the index given by the user matches our camera index
 			if (nr_cam == index)
 			{
+				dev->usb_cam.VID = desc.idVendor;
+				dev->usb_cam.PID = desc.idProduct;
+
 				res = libusb_open (devs[i], &dev->usb_cam.dev);
 				if (res < 0 || !dev->usb_cam.dev)
 				{
@@ -416,6 +419,9 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
 			// If the index given by the user matches our camera index
 			if (nr_mot == index)
 			{
+				dev->usb_motor.VID = desc.idVendor;
+				dev->usb_motor.PID = desc.idProduct;
+
 				res = libusb_open (devs[i], &dev->usb_motor.dev);
 				if (res < 0 || !dev->usb_motor.dev)
 				{
@@ -444,6 +450,9 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
 			// If the index given by the user matches our audio index
 			if (nr_audio == index)
 			{
+				dev->usb_audio.VID = desc.idVendor;
+				dev->usb_audio.PID = desc.idProduct;
+
 				res = libusb_open (devs[i], &dev->usb_audio.dev);
 				if (res < 0 || !dev->usb_audio.dev)
 				{

src/usb_libusb10.h

@@ -31,15 +31,7 @@
 
 #if defined(__APPLE__)
 /*
-  From Github Issue 22 by Roefer -
-  https://github.com/OpenKinect/libfreenect/issues/#issue/22
-
-  The current implementation still does not reach 30 Hz on MacOS. This
-  is due to bad scheduling of USB transfers in libusb (Ed Note: libusb
-  1.0.8). A fix can be found at
-  http://www.informatik.uni-bremen.de/~roefer/libusb/libusb-osx-kinect.diff
-
-  (Ed Note: patch applies to libusb repo at 7da756e09fd)
+  https://github.com/OpenKinect/libfreenect/issues/22 (@Roefer)
 
   In camera.c, I use PKTS_PER_XFER = 128, NUM_XFERS = 4. There are a
   few rules: PKTS_PER_XFER * NUM_XFERS <= 1000, PKTS_PER_XFER % 8 == 0.
@@ -69,6 +61,8 @@ typedef struct {
 	freenect_device *parent; //so we can go up from the libusb userdata
 	libusb_device_handle *dev;
 	int device_dead; // set to 1 when the underlying libusb_device_handle vanishes (ie, Kinect was unplugged)
+	int VID;
+	int PID;
 } fnusb_dev;
 
 typedef struct {