Skip to content

Commit dcb2929

Browse files
piedarpiedar
committed
Allow execution to continue when the audio device cannot be loaded.
Signed-off-by: Benn Snyder <[email protected]>
1 parent 89f77f6 commit dcb2929

File tree

3 files changed

+132
-96
lines changed

3 files changed

+132
-96
lines changed

‎src/core.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ FREENECTAPI int freenect_init(freenect_context **ctx, freenect_usb_context *usb_
4848
memset(*ctx, 0, sizeof(freenect_context));
4949

5050
(*ctx)->log_level = LL_WARNING;
51-
(*ctx)->enabled_subdevices = (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA | FREENECT_DEVICE_AUDIO);
51+
(*ctx)->enabled_subdevices = (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA);
5252
res = fnusb_init(&(*ctx)->usb, usb_ctx);
5353
if (res < 0) {
5454
free(*ctx);

‎src/tilt.c‎

Lines changed: 45 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -97,9 +97,16 @@ freenect_raw_tilt_state* freenect_get_tilt_state(freenect_device *dev)
9797
return &dev->raw_state;
9898
}
9999

100-
int update_tilt_state_alt(freenect_device *dev){
100+
int update_tilt_state_alt(freenect_device *dev)
101+
{
101102
freenect_context *ctx = dev->parent;
102103

104+
if (dev->usb_audio.dev == NULL)
105+
{
106+
FN_WARNING("Motor control failed: audio device missing");
107+
return -1;
108+
}
109+
103110
int transferred = 0;
104111
int res = 0;
105112
fn_alt_motor_command cmd;
@@ -112,39 +119,34 @@ int update_tilt_state_alt(freenect_device *dev){
112119
memcpy(buffer, &cmd, 16);
113120

114121
res = libusb_bulk_transfer(dev->usb_audio.dev, 0x01, buffer, 16, &transferred, 250);
115-
if (res != 0) {
122+
if (res != 0)
123+
{
116124
return res;
117125
}
118126

119127
res = libusb_bulk_transfer(dev->usb_audio.dev, 0x81, buffer, 256, &transferred, 250); // 104 bytes
120-
if (res != 0) {
128+
if (res != 0)
129+
{
121130
return res;
122-
} else {
123-
// int i;
124-
// for(i = 0 ; i < transferred ; i += 4) {
125-
// int32_t j;
126-
// memcpy(&j, buffer + i, 4);
127-
// printf("\t%d\n", j);
128-
// }
129-
// printf("\n");
130-
struct {
131-
int32_t x;
132-
int32_t y;
133-
int32_t z;
134-
int32_t tilt;
135-
} accel_and_tilt;
136-
137-
memcpy(&accel_and_tilt, buffer + 16, sizeof(accel_and_tilt));
138-
//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);
139-
140-
dev->raw_state.accelerometer_x = (int16_t)accel_and_tilt.x;
141-
dev->raw_state.accelerometer_y = (int16_t)accel_and_tilt.y;
142-
dev->raw_state.accelerometer_z = (int16_t)accel_and_tilt.z;
143-
144-
//this is multiplied by 2 as the older 1414 device reports angles doubled and freenect takes this into account
145-
dev->raw_state.tilt_angle = (int8_t)accel_and_tilt.tilt * 2;
146-
147131
}
132+
133+
struct {
134+
int32_t x;
135+
int32_t y;
136+
int32_t z;
137+
int32_t tilt;
138+
} accel_and_tilt;
139+
140+
memcpy(&accel_and_tilt, buffer + 16, sizeof(accel_and_tilt));
141+
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);
142+
143+
dev->raw_state.accelerometer_x = (int16_t)accel_and_tilt.x;
144+
dev->raw_state.accelerometer_y = (int16_t)accel_and_tilt.y;
145+
dev->raw_state.accelerometer_z = (int16_t)accel_and_tilt.z;
146+
147+
// this is multiplied by 2 as the older 1414 device reports angles doubled and freenect takes this into account
148+
dev->raw_state.tilt_angle = (int8_t)accel_and_tilt.tilt * 2;
149+
148150
// Reply: skip four uint32_t, then you have three int32_t that give you acceleration in that direction, it seems.
149151
// Units still to be worked out.
150152
return get_reply(dev->usb_audio.dev, ctx);
@@ -192,7 +194,13 @@ int freenect_set_tilt_degs_alt(freenect_device *dev, int tilt_degrees)
192194
FN_WARNING("set_tilt(): degrees %d out of safe range [-31, 31]\n", tilt_degrees);
193195
return -1;
194196
}
195-
197+
198+
if (dev->usb_audio.dev == NULL)
199+
{
200+
FN_WARNING("Motor control failed: audio device missing");
201+
return -1;
202+
}
203+
196204
fn_alt_motor_command cmd;
197205
cmd.magic = fn_le32(0x06022009);
198206
cmd.tag = fn_le32(tag_seq++);
@@ -282,7 +290,14 @@ FN_INTERNAL int fnusb_set_led_alt(libusb_device_handle * dev, freenect_context *
282290
int freenect_set_led_alt(freenect_device *dev, freenect_led_options state)
283291
{
284292
freenect_context *ctx = dev->parent;
285-
return fnusb_set_led_alt(dev->usb_audio.dev, ctx, state);
293+
294+
if (dev->usb_audio.dev == NULL)
295+
{
296+
FN_WARNING("Motor control failed: audio device missing");
297+
return -1;
298+
}
299+
300+
return fnusb_set_led_alt(dev->usb_audio.dev, ctx, state);
286301
}
287302

288303
int freenect_set_led(freenect_device *dev, freenect_led_options option)

‎src/usb_libusb10.c‎

Lines changed: 86 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -252,6 +252,53 @@ FN_INTERNAL int fnusb_process_events_timeout(fnusb_ctx *ctx, struct timeval* tim
252252
return libusb_handle_events_timeout(ctx->ctx, timeout);
253253
}
254254

255+
int fnusb_claim_camera(freenect_device* dev)
256+
{
257+
freenect_context *ctx = dev->parent;
258+
259+
int ret = 0;
260+
261+
#ifndef _WIN32 // todo: necessary?
262+
// Detach an existing kernel driver for the device
263+
ret = libusb_kernel_driver_active(dev->usb_cam.dev, 0);
264+
if (ret == 1)
265+
{
266+
ret = libusb_detach_kernel_driver(dev->usb_cam.dev, 0);
267+
if (ret < 0)
268+
{
269+
FN_ERROR("Failed to detach camera kernel driver: %s\n", libusb_error_name(ret));
270+
libusb_close(dev->usb_cam.dev);
271+
dev->usb_cam.dev = NULL;
272+
return ret;
273+
}
274+
}
275+
#endif
276+
277+
ret = libusb_claim_interface(dev->usb_cam.dev, 0);
278+
if (ret < 0)
279+
{
280+
FN_ERROR("Failed to claim camera interface: %s\n", libusb_error_name(ret));
281+
libusb_close(dev->usb_cam.dev);
282+
dev->usb_cam.dev = NULL;
283+
return ret;
284+
}
285+
286+
if (dev->usb_cam.PID == PID_K4W_CAMERA)
287+
{
288+
ret = libusb_set_interface_alt_setting(dev->usb_cam.dev, 0, 1);
289+
if (ret != 0)
290+
{
291+
FN_ERROR("Failed to set alternate interface #1 for K4W: %s\n", libusb_error_name(ret));
292+
libusb_close(dev->usb_cam.dev);
293+
dev->usb_cam.dev = NULL;
294+
return ret;
295+
}
296+
}
297+
298+
return ret;
299+
}
300+
301+
255302
FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
256303
{
257304
freenect_context *ctx = dev->parent;
@@ -301,6 +348,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
301348
dev->usb_cam.dev = NULL;
302349
break;
303350
}
351+
304352
if (desc.idProduct == PID_K4W_CAMERA || desc.bcdDevice != fn_le32(267))
305353
{
306354
freenect_device_flags requested_devices = ctx->enabled_subdevices;
@@ -309,7 +357,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
309357
ctx->enabled_subdevices = (freenect_device_flags)(ctx->enabled_subdevices & ~FREENECT_DEVICE_MOTOR);
310358

311359
ctx->zero_plane_res = 334;
312-
dev->device_does_motor_control_with_audio = 1;
360+
dev->device_does_motor_control_with_audio = 1;
313361

314362
// set the LED for non 1414 devices to keep the camera alive for some systems which get freezes
315363

@@ -360,40 +408,11 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
360408
ctx->zero_plane_res = 322;
361409
}
362410

363-
#ifndef _WIN32
364-
// Detach an existing kernel driver for the device
365-
res = libusb_kernel_driver_active(dev->usb_cam.dev, 0);
366-
if (res == 1)
367-
{
368-
res = libusb_detach_kernel_driver(dev->usb_cam.dev, 0);
369-
if (res < 0)
370-
{
371-
FN_ERROR("Could not detach kernel driver for camera: %d\n", res);
372-
libusb_close(dev->usb_cam.dev);
373-
dev->usb_cam.dev = NULL;
374-
break;
375-
}
376-
}
377-
#endif
378-
res = libusb_claim_interface (dev->usb_cam.dev, 0);
411+
res = fnusb_claim_camera(dev);
379412
if (res < 0)
380413
{
381-
FN_ERROR("Could not claim interface on camera: %d\n", res);
382-
libusb_close(dev->usb_cam.dev);
383-
dev->usb_cam.dev = NULL;
384414
break;
385415
}
386-
if (desc.idProduct == PID_K4W_CAMERA)
387-
{
388-
res = libusb_set_interface_alt_setting(dev->usb_cam.dev, 0, 1);
389-
if (res != 0)
390-
{
391-
FN_ERROR("Failed to set alternate interface #1 for K4W: %d\n", res);
392-
libusb_close(dev->usb_cam.dev);
393-
dev->usb_cam.dev = NULL;
394-
break;
395-
}
396-
}
397416
}
398417
else
399418
{
@@ -460,6 +479,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
460479
dev->usb_audio.dev = NULL;
461480
break;
462481
}
482+
463483
res = libusb_claim_interface (dev->usb_audio.dev, 0);
464484
if (res < 0)
465485
{
@@ -569,7 +589,7 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
569589
// Save the device handle.
570590
dev->usb_audio.dev = new_dev_handle;
571591

572-
// Verify that we've actually found a device running the right firmware.
592+
// Verify that we've actually found a device running the right firmware.
573593
num_interfaces = fnusb_num_interfaces(&dev->usb_audio);
574594

575595
if (num_interfaces >= 2)
@@ -585,7 +605,9 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
585605
dev->usb_audio.dev = NULL;
586606
libusb_close(new_dev_handle);
587607
continue;
588-
} break;
608+
}
609+
610+
break;
589611
}
590612
else
591613
{
@@ -613,47 +635,46 @@ FN_INTERNAL int fnusb_open_subdevices(freenect_device *dev, int index)
613635

614636
libusb_free_device_list (devs, 1); // free the list, unref the devices in it
615637

616-
// Check that each subdevice is either opened or not enabled.
617638
if ((dev->usb_cam.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_CAMERA))
618-
&& (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR))
619-
&& (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO)))
639+
&& (dev->usb_motor.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_MOTOR)))
640+
//&& (dev->usb_audio.dev || !(ctx->enabled_subdevices & FREENECT_DEVICE_AUDIO)))
620641
{
642+
// Each requested subdevice is open.
643+
// Except audio, which may fail if firmware is missing (or because it hates us).
621644
return 0;
622645
}
646+
647+
if (dev->usb_cam.dev != NULL)
648+
{
649+
libusb_release_interface(dev->usb_cam.dev, 0);
650+
libusb_close(dev->usb_cam.dev);
651+
}
623652
else
624653
{
625-
if (dev->usb_cam.dev)
626-
{
627-
libusb_release_interface(dev->usb_cam.dev, 0);
628-
libusb_close(dev->usb_cam.dev);
629-
}
630-
else
631-
{
632-
FN_ERROR("Failed to open camera subdevice or it is not disabled.");
633-
}
634-
635-
if (dev->usb_motor.dev)
636-
{
637-
libusb_release_interface(dev->usb_motor.dev, 0);
638-
libusb_close(dev->usb_motor.dev);
639-
}
640-
else
641-
{
642-
FN_ERROR("Failed to open motor subddevice or it is not disabled.");
643-
}
654+
FN_ERROR("Failed to open camera subdevice or it is not disabled.");
655+
}
644656

645-
if (dev->usb_audio.dev)
646-
{
647-
libusb_release_interface(dev->usb_audio.dev, 0);
648-
libusb_close(dev->usb_audio.dev);
649-
}
650-
else
651-
{
652-
FN_ERROR("Failed to open audio subdevice or it is not disabled.");
653-
}
657+
if (dev->usb_motor.dev != NULL)
658+
{
659+
libusb_release_interface(dev->usb_motor.dev, 0);
660+
libusb_close(dev->usb_motor.dev);
661+
}
662+
else
663+
{
664+
FN_ERROR("Failed to open motor subddevice or it is not disabled.");
665+
}
654666

655-
return -1;
667+
if (dev->usb_audio.dev != NULL)
668+
{
669+
libusb_release_interface(dev->usb_audio.dev, 0);
670+
libusb_close(dev->usb_audio.dev);
656671
}
672+
else
673+
{
674+
FN_ERROR("Failed to open audio subdevice or it is not disabled.");
675+
}
676+
677+
return -1;
657678
}
658679

659680
FN_INTERNAL int fnusb_close_subdevices(freenect_device *dev)

0 commit comments

Comments
 (0)