diff --git a/src/core.c b/src/core.c index 006152a6..edaba269 100644 --- a/src/core.c +++ b/src/core.c @@ -48,7 +48,7 @@ FREENECTAPI int freenect_init(freenect_context **ctx, freenect_usb_context *usb_ memset(*ctx, 0, sizeof(freenect_context)); (*ctx)->log_level = LL_WARNING; - (*ctx)->enabled_subdevices = (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA | FREENECT_DEVICE_AUDIO); + (*ctx)->enabled_subdevices = (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA); res = fnusb_init(&(*ctx)->usb, usb_ctx); if (res < 0) { free(*ctx); diff --git a/src/tilt.c b/src/tilt.c index 4015607a..43aa537d 100644 --- a/src/tilt.c +++ b/src/tilt.c @@ -97,9 +97,16 @@ freenect_raw_tilt_state* freenect_get_tilt_state(freenect_device *dev) return &dev->raw_state; } -int update_tilt_state_alt(freenect_device *dev){ +int update_tilt_state_alt(freenect_device *dev) +{ freenect_context *ctx = dev->parent; + if (dev->usb_audio.dev == NULL) + { + FN_WARNING("Motor control failed: audio device missing"); + return -1; + } + int transferred = 0; int res = 0; fn_alt_motor_command cmd; @@ -112,39 +119,34 @@ int update_tilt_state_alt(freenect_device *dev){ memcpy(buffer, &cmd, 16); res = libusb_bulk_transfer(dev->usb_audio.dev, 0x01, buffer, 16, &transferred, 250); - if (res != 0) { + if (res != 0) + { return res; } res = libusb_bulk_transfer(dev->usb_audio.dev, 0x81, buffer, 256, &transferred, 250); // 104 bytes - if (res != 0) { + if (res != 0) + { return res; - } else { -// int i; -// for(i = 0 ; i < transferred ; i += 4) { -// int32_t j; -// memcpy(&j, buffer + i, 4); -// printf("\t%d\n", j); -// } -// printf("\n"); - struct { - int32_t x; - int32_t y; - int32_t z; - int32_t tilt; - } accel_and_tilt; - - memcpy(&accel_and_tilt, buffer + 16, sizeof(accel_and_tilt)); - //printf("\tX: %d Y: %d Z:%d - tilt is %d\n", accel_and_tilt.x, accel_and_tilt.y, accel_and_tilt.z, accel_and_tilt.tilt); - - dev->raw_state.accelerometer_x = (int16_t)accel_and_tilt.x; - dev->raw_state.accelerometer_y = (int16_t)accel_and_tilt.y; - dev->raw_state.accelerometer_z = (int16_t)accel_and_tilt.z; - - //this is multiplied by 2 as the older 1414 device reports angles doubled and freenect takes this into account - dev->raw_state.tilt_angle = (int8_t)accel_and_tilt.tilt * 2; - } + + struct { + int32_t x; + int32_t y; + int32_t z; + int32_t tilt; + } accel_and_tilt; + + memcpy(&accel_and_tilt, buffer + 16, sizeof(accel_and_tilt)); + FN_SPEW("Accelerometer state: X == %d \t Y == %d \t Z == %d \t Tilt == %d\n", accel_and_tilt.x, accel_and_tilt.y, accel_and_tilt.z, accel_and_tilt.tilt); + + dev->raw_state.accelerometer_x = (int16_t)accel_and_tilt.x; + dev->raw_state.accelerometer_y = (int16_t)accel_and_tilt.y; + dev->raw_state.accelerometer_z = (int16_t)accel_and_tilt.z; + + // this is multiplied by 2 as the older 1414 device reports angles doubled and freenect takes this into account + dev->raw_state.tilt_angle = (int8_t)accel_and_tilt.tilt * 2; + // Reply: skip four uint32_t, then you have three int32_t that give you acceleration in that direction, it seems. // Units still to be worked out. return get_reply(dev->usb_audio.dev, ctx); @@ -192,7 +194,13 @@ int freenect_set_tilt_degs_alt(freenect_device *dev, int tilt_degrees) FN_WARNING("set_tilt(): degrees %d out of safe range [-31, 31]\n", tilt_degrees); return -1; } - + + if (dev->usb_audio.dev == NULL) + { + FN_WARNING("Motor control failed: audio device missing"); + return -1; + } + fn_alt_motor_command cmd; cmd.magic = fn_le32(0x06022009); cmd.tag = fn_le32(tag_seq++); @@ -282,7 +290,14 @@ FN_INTERNAL int fnusb_set_led_alt(libusb_device_handle * dev, freenect_context * int freenect_set_led_alt(freenect_device *dev, freenect_led_options state) { freenect_context *ctx = dev->parent; - return fnusb_set_led_alt(dev->usb_audio.dev, ctx, state); + + if (dev->usb_audio.dev == NULL) + { + FN_WARNING("Motor control failed: audio device missing"); + return -1; + } + + return fnusb_set_led_alt(dev->usb_audio.dev, ctx, state); } int freenect_set_led(freenect_device *dev, freenect_led_options option) diff --git a/src/usb_libusb10.c b/src/usb_libusb10.c index 43f1443e..751d9a91 100644 --- a/src/usb_libusb10.c +++ b/src/usb_libusb10.c @@ -252,6 +252,53 @@ FN_INTERNAL int fnusb_process_events_timeout(fnusb_ctx *ctx, struct timeval* tim return libusb_handle_events_timeout(ctx->ctx, timeout); } +int fnusb_claim_camera(freenect_device* dev) +{ + freenect_context *ctx = dev->parent; + + int ret = 0; + +#ifndef _WIN32 // todo: necessary? + // Detach an existing kernel driver for the device + ret = libusb_kernel_driver_active(dev->usb_cam.dev, 0); + if (ret == 1) + { + ret = libusb_detach_kernel_driver(dev->usb_cam.dev, 0); + if (ret < 0) + { + FN_ERROR("Failed to detach camera kernel driver: %s\n", libusb_error_name(ret)); + libusb_close(dev->usb_cam.dev); + dev->usb_cam.dev = NULL; + return ret; + } + } +#endif + + ret = libusb_claim_interface(dev->usb_cam.dev, 0); + if (ret < 0) + { + FN_ERROR("Failed to claim camera interface: %s\n", libusb_error_name(ret)); + libusb_close(dev->usb_cam.dev); + dev->usb_cam.dev = NULL; + return ret; + } + + if (dev->usb_cam.PID == PID_K4W_CAMERA) + { + ret = libusb_set_interface_alt_setting(dev->usb_cam.dev, 0, 1); + if (ret != 0) + { + FN_ERROR("Failed to set alternate interface #1 for K4W: %s\n", libusb_error_name(ret)); + libusb_close(dev->usb_cam.dev); + dev->usb_cam.dev = NULL; + return ret; + } + } + + return ret; +} + + FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) { freenect_context *ctx = dev->parent; @@ -301,6 +348,7 @@ 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)) { freenect_device_flags requested_devices = ctx->enabled_subdevices; @@ -309,7 +357,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) ctx->enabled_subdevices = (freenect_device_flags)(ctx->enabled_subdevices & ~FREENECT_DEVICE_MOTOR); ctx->zero_plane_res = 334; - dev->device_does_motor_control_with_audio = 1; + dev->device_does_motor_control_with_audio = 1; // set the LED for non 1414 devices to keep the camera alive for some systems which get freezes @@ -360,40 +408,11 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) 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); - if (res == 1) - { - res = libusb_detach_kernel_driver(dev->usb_cam.dev, 0); - if (res < 0) - { - FN_ERROR("Could not detach kernel driver for camera: %d\n", res); - libusb_close(dev->usb_cam.dev); - dev->usb_cam.dev = NULL; - break; - } - } -#endif - res = libusb_claim_interface (dev->usb_cam.dev, 0); + res = fnusb_claim_camera(dev); if (res < 0) { - FN_ERROR("Could not claim interface on camera: %d\n", res); - libusb_close(dev->usb_cam.dev); - 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 { @@ -460,6 +479,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) dev->usb_audio.dev = NULL; break; } + res = libusb_claim_interface (dev->usb_audio.dev, 0); if (res < 0) { @@ -569,7 +589,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) // Save the device handle. dev->usb_audio.dev = new_dev_handle; - // Verify that we've actually found a device running the right firmware. + // Verify that we've actually found a device running the right firmware. num_interfaces = fnusb_num_interfaces(&dev->usb_audio); if (num_interfaces >= 2) @@ -585,7 +605,9 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) dev->usb_audio.dev = NULL; libusb_close(new_dev_handle); continue; - } break; + } + + break; } else { @@ -613,47 +635,46 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index) libusb_free_device_list (devs, 1); // free the list, unref the devices in it - // Check that each subdevice is either opened or not enabled. if ((dev->usb_cam.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA)) - && (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)) - && (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO))) + && (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR))) + //&& (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO))) { + // Each requested subdevice is open. + // Except audio, which may fail if firmware is missing (or because it hates us). return 0; } + + if (dev->usb_cam.dev != NULL) + { + libusb_release_interface(dev->usb_cam.dev, 0); + libusb_close(dev->usb_cam.dev); + } else { - if (dev->usb_cam.dev) - { - libusb_release_interface(dev->usb_cam.dev, 0); - libusb_close(dev->usb_cam.dev); - } - else - { - FN_ERROR("Failed to open camera subdevice or it is not disabled."); - } - - if (dev->usb_motor.dev) - { - libusb_release_interface(dev->usb_motor.dev, 0); - libusb_close(dev->usb_motor.dev); - } - else - { - FN_ERROR("Failed to open motor subddevice or it is not disabled."); - } + FN_ERROR("Failed to open camera subdevice or it is not disabled."); + } - if (dev->usb_audio.dev) - { - libusb_release_interface(dev->usb_audio.dev, 0); - libusb_close(dev->usb_audio.dev); - } - else - { - FN_ERROR("Failed to open audio subdevice or it is not disabled."); - } + if (dev->usb_motor.dev != NULL) + { + libusb_release_interface(dev->usb_motor.dev, 0); + libusb_close(dev->usb_motor.dev); + } + else + { + FN_ERROR("Failed to open motor subddevice or it is not disabled."); + } - return -1; + if (dev->usb_audio.dev != NULL) + { + libusb_release_interface(dev->usb_audio.dev, 0); + libusb_close(dev->usb_audio.dev); } + else + { + FN_ERROR("Failed to open audio subdevice or it is not disabled."); + } + + return -1; } FN_INTERNAL int fnusb_close_subdevices(freenect_device *dev)