1 #include <stdio.h>

2 #include <pthread.h>

3 #include <libfreenect.h>

4

5 #include "kinect.h"

6 #include "frame.h"

7

8 bool init_kinect(freenect_context **kin_ctx, freenect_device **kin_dev, KinectParams *kin_params)

9 {

10 if(freenect_init(kin_ctx, NULL) < 0) {

11 fprintf(stderr, "Failed to initialize kinect context.\n");

12 return false;

13 }

14

15 freenect_set_log_level(*kin_ctx, FREENECT_LOG_INFO);

16

17 int num_dev = freenect_num_devices(*kin_ctx);

18 if(num_dev < 1) {

19 fprintf(stderr, "Kinect device not found.\n");

20 return false;

21 }

22 printf("%d devices found, will use the first.\n", num_dev);

23

24 if(freenect_open_device(*kin_ctx, kin_dev, 0) < 0) {

25 fprintf(stderr, "Couldn't open kinect device.\n");

26 return false;

27 }

28

29 if(freenect_set_tilt_degs(*kin_dev, kin_params->angle) < 0) {

30 stop_kinect(*kin_ctx, *kin_dev);

31 fprintf(stderr, "Failed to set kinect angle\n");

32 return false;

33 }

34

35 if(freenect_set_led(*kin_dev, kin_params->led_color) < 0) {

36 stop_kinect(*kin_ctx, *kin_dev);

37 fprintf(stderr, "Failed to set kinect LED\n");

38 return false;

39 }

40

41 init_frame_mx();

42

43 freenect_set_video_callback(*kin_dev, video_cb);

44 freenect_set_depth_callback(*kin_dev, depth_cb);

45

46 if(freenect_set_video_mode(*kin_dev,

47 freenect_find_video_mode(kin_params->video_res,

48 kin_params->video_format)) < 0) {

49 stop_kinect(*kin_ctx, *kin_dev);

50 fprintf(stderr, "Failed to set kinect video mode.\n");

51 return false;

52 }

53

54 if(freenect_set_depth_mode(*kin_dev,

55 freenect_find_depth_mode(kin_params->depth_res,

56 kin_params->depth_format)) < 0) {

57 stop_kinect(*kin_ctx, *kin_dev);

58 fprintf(stderr, "Failed to set kinect depth mode.\n");

59 return false;

60 }

61

62 if(freenect_start_video(*kin_dev) < 0) {

63 fprintf(stderr, "Failed to start kinect video.\n");

64 return false;

65 }

66

67 if(freenect_start_depth(*kin_dev) < 0) {

68 fprintf(stderr, "Failed to start kinect depth video.\n");

69 return false;

70 }

71

72 return true;

73 }

74

75 void stop_kinect(freenect_context *kin_ctx, freenect_device *kin_dev)

76 {

77 freenect_set_led(kin_dev, LED_OFF);

78

79 freenect_stop_depth(kin_dev);

80 freenect_stop_video(kin_dev);

81 freenect_close_device(kin_dev);

82 freenect_shutdown(kin_ctx);

83 }