Skip to content

Commit

Permalink
Allow execution to continue when the audio device cannot be loaded.
Browse files Browse the repository at this point in the history 
Signed-off-by: Benn Snyder <benn.snyder@gmail.com>
  • Loading branch information
@piedar
piedar committed Aug 5, 2015
1 parent 89f77f6 commit dcb2929
Show file tree
Hide file tree
Showing 3 changed files with 132 additions and 96 deletions.
2 changes: 1 addition & 1 deletion src/core.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
75 changes: 45 additions & 30 deletions src/tilt.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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++);
Expand Down Expand Up @@ -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)
Expand Down
151 changes: 86 additions & 65 deletions src/usb_libusb10.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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

Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
Expand All @@ -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
{
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit dcb2929

Please sign in to comment.