diff --git a/wrappers/c_sync/libfreenect_sync.c b/wrappers/c_sync/libfreenect_sync.c index 6fe55bd7..34ecfd94 100644 --- a/wrappers/c_sync/libfreenect_sync.c +++ b/wrappers/c_sync/libfreenect_sync.c @@ -37,6 +37,7 @@ typedef struct buffer_ring { uint32_t timestamp; int valid; // True if middle buffer is valid int fmt; + int res; } buffer_ring_t; typedef struct sync_kinect { @@ -67,7 +68,7 @@ static pthread_cond_t pending_runloop_tasks_cond = PTHREAD_COND_INITIALIZER; - runloop_lock, buffer_ring_t.lock (NOTE: You may only have one) */ -static int alloc_buffer_ring_video(freenect_video_format fmt, buffer_ring_t *buf) +static int alloc_buffer_ring_video(freenect_resolution res, freenect_video_format fmt, buffer_ring_t *buf) { int sz, i; switch (fmt) { @@ -76,7 +77,7 @@ static int alloc_buffer_ring_video(freenect_video_format fmt, buffer_ring_t *buf case FREENECT_VIDEO_IR_8BIT: case FREENECT_VIDEO_IR_10BIT: case FREENECT_VIDEO_IR_10BIT_PACKED: - sz = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, fmt).bytes; + sz = freenect_find_video_mode(res, fmt).bytes; break; default: printf("Invalid video format %d\n", fmt); @@ -87,10 +88,11 @@ static int alloc_buffer_ring_video(freenect_video_format fmt, buffer_ring_t *buf buf->timestamp = 0; buf->valid = 0; buf->fmt = fmt; + buf->res = res; return 0; } -static int alloc_buffer_ring_depth(freenect_depth_format fmt, buffer_ring_t *buf) +static int alloc_buffer_ring_depth(freenect_resolution res, freenect_depth_format fmt, buffer_ring_t *buf) { int sz, i; switch (fmt) { @@ -100,7 +102,7 @@ static int alloc_buffer_ring_depth(freenect_depth_format fmt, buffer_ring_t *buf case FREENECT_DEPTH_10BIT_PACKED: case FREENECT_DEPTH_REGISTERED: case FREENECT_DEPTH_MM: - sz = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, fmt).bytes; + sz = freenect_find_depth_mode(res, fmt).bytes; break; default: printf("Invalid depth format %d\n", fmt); @@ -111,6 +113,7 @@ static int alloc_buffer_ring_depth(freenect_depth_format fmt, buffer_ring_t *buf buf->timestamp = 0; buf->valid = 0; buf->fmt = fmt; + buf->res = res; return 0; } @@ -124,6 +127,7 @@ static void free_buffer_ring(buffer_ring_t *buf) buf->timestamp = 0; buf->valid = 0; buf->fmt = -1; + buf->res = -1; } static void producer_cb_inner(freenect_device *dev, void *data, uint32_t timestamp, buffer_ring_t *buf, set_buffer_t set_buffer) @@ -217,30 +221,40 @@ static void init_thread(void) pthread_create(&thread, NULL, init, NULL); } -static int change_video_format(sync_kinect_t *kinect, freenect_video_format fmt) +static int change_video_format_with_res(sync_kinect_t *kinect, freenect_resolution res, freenect_video_format fmt) { freenect_stop_video(kinect->dev); free_buffer_ring(&kinect->video); - if (alloc_buffer_ring_video(fmt, &kinect->video)) + if (alloc_buffer_ring_video(res, fmt, &kinect->video)) return -1; - freenect_set_video_mode(kinect->dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, fmt)); + freenect_set_video_mode(kinect->dev, freenect_find_video_mode(res, fmt)); freenect_set_video_buffer(kinect->dev, kinect->video.bufs[2]); freenect_start_video(kinect->dev); return 0; } -static int change_depth_format(sync_kinect_t *kinect, freenect_depth_format fmt) +static int change_video_format(sync_kinect_t *kinect, freenect_resolution res, freenect_video_format fmt) +{ + return change_video_format_with_res(kinect, FREENECT_RESOLUTION_MEDIUM, fmt); +} + +static int change_depth_format_with_res(sync_kinect_t *kinect, freenect_resolution res, freenect_depth_format fmt) { freenect_stop_depth(kinect->dev); free_buffer_ring(&kinect->depth); - if (alloc_buffer_ring_depth(fmt, &kinect->depth)) + if (alloc_buffer_ring_depth(res, fmt, &kinect->depth)) return -1; - freenect_set_depth_mode(kinect->dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, fmt)); + freenect_set_depth_mode(kinect->dev, freenect_find_depth_mode(res, fmt)); freenect_set_depth_buffer(kinect->dev, kinect->depth.bufs[2]); freenect_start_depth(kinect->dev); return 0; } +static int change_depth_format(sync_kinect_t *kinect, freenect_resolution res, freenect_depth_format fmt) +{ + return change_depth_format_with_res(kinect, FREENECT_RESOLUTION_MEDIUM, fmt); +} + static sync_kinect_t *alloc_kinect(int index) { sync_kinect_t *kinect = (sync_kinect_t*)malloc(sizeof(sync_kinect_t)); @@ -254,7 +268,9 @@ static sync_kinect_t *alloc_kinect(int index) kinect->depth.bufs[i] = NULL; } kinect->video.fmt = -1; + kinect->video.res = -1; kinect->depth.fmt = -1; + kinect->depth.res = -1; freenect_set_video_callback(kinect->dev, video_producer_cb); freenect_set_depth_callback(kinect->dev, depth_producer_cb); pthread_mutex_init(&kinect->video.lock, NULL); @@ -264,7 +280,7 @@ static sync_kinect_t *alloc_kinect(int index) return kinect; } -static int setup_kinect(int index, int fmt, int is_depth) +static int setup_kinect_with_res(int index, int res, int fmt, int is_depth) { pending_runloop_tasks_inc(); pthread_mutex_lock(&runloop_lock); @@ -295,18 +311,22 @@ static int setup_kinect(int index, int fmt, int is_depth) else buf = &kinects[index]->video; pthread_mutex_lock(&buf->lock); - if (buf->fmt != fmt) { + if ((buf->fmt != fmt) || (buf->res != res)) { if (is_depth) - change_depth_format(kinects[index], (freenect_depth_format)fmt); + change_depth_format_with_res(kinects[index], (freenect_resolution)res, (freenect_depth_format)fmt); else - change_video_format(kinects[index], (freenect_video_format)fmt); - } + change_video_format_with_res(kinects[index], (freenect_resolution)res, (freenect_video_format)fmt); + } pthread_mutex_unlock(&buf->lock); pthread_mutex_unlock(&runloop_lock); pending_runloop_tasks_dec(); return 0; } +static int setup_kinect(int index, int fmt, int is_depth) { + return setup_kinect_with_res(index, FREENECT_RESOLUTION_MEDIUM, fmt, is_depth); +} + static int sync_get(void **data, uint32_t *timestamp, buffer_ring_t *buf) { pthread_mutex_lock(&buf->lock); @@ -339,7 +359,7 @@ static int runloop_enter(int index) return -1; } if (!thread_running || !kinects[index]) - if (setup_kinect(index, FREENECT_DEPTH_11BIT, 1)) + if (setup_kinect_with_res(index, FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT, 1)) return -1; pending_runloop_tasks_inc(); @@ -353,32 +373,46 @@ static void runloop_exit() pending_runloop_tasks_dec(); } -int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freenect_video_format fmt) +int freenect_sync_get_video_with_res(void **video, uint32_t *timestamp, int index, + freenect_resolution res, freenect_video_format fmt) { if (index < 0 || index >= MAX_KINECTS) { printf("Error: Invalid index [%d]\n", index); return -1; } - if (!thread_running || !kinects[index] || kinects[index]->video.fmt != fmt) - if (setup_kinect(index, fmt, 0)) + if (!thread_running || !kinects[index] || kinects[index]->video.fmt != fmt + || kinects[index]->video.res != res) + if (setup_kinect_with_res(index, res, fmt, 0)) return -1; sync_get(video, timestamp, &kinects[index]->video); return 0; } -int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freenect_depth_format fmt) +int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freenect_video_format fmt) +{ + return freenect_sync_get_video_with_res(video, timestamp, index, FREENECT_RESOLUTION_MEDIUM, fmt); +} + +int freenect_sync_get_depth_with_res(void **depth, uint32_t *timestamp, int index, + freenect_resolution res, freenect_depth_format fmt) { if (index < 0 || index >= MAX_KINECTS) { printf("Error: Invalid index [%d]\n", index); return -1; } - if (!thread_running || !kinects[index] || kinects[index]->depth.fmt != fmt) - if (setup_kinect(index, fmt, 1)) + if (!thread_running || !kinects[index] || kinects[index]->depth.fmt != fmt + || kinects[index]->depth.res != res) + if (setup_kinect_with_res(index, res, fmt, 1)) return -1; sync_get(depth, timestamp, &kinects[index]->depth); return 0; } +int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freenect_depth_format fmt) +{ + return freenect_sync_get_depth_with_res(depth, timestamp, index, FREENECT_RESOLUTION_MEDIUM, fmt); +} + int freenect_sync_get_tilt_state(freenect_raw_tilt_state **state, int index) { if (runloop_enter(index)) return -1; diff --git a/wrappers/c_sync/libfreenect_sync.h b/wrappers/c_sync/libfreenect_sync.h index 35e76c40..ef2680e0 100644 --- a/wrappers/c_sync/libfreenect_sync.h +++ b/wrappers/c_sync/libfreenect_sync.h @@ -33,7 +33,8 @@ extern "C" { #endif -int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freenect_video_format fmt); +int freenect_sync_get_video_with_res(void **video, uint32_t *timestamp, int index, + freenect_resolution res, freenect_video_format fmt); /* Synchronous video function, starts the runloop if it isn't running The returned buffer is valid until this function is called again, after which the buffer must not @@ -43,14 +44,21 @@ int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freene video: Populated with a pointer to a video buffer with a size of the requested type timestamp: Populated with the associated timestamp index: Device index (0 is the first) + res: Valid resolution fmt: Valid format Returns: Nonzero on error. */ +int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freenect_video_format fmt); +/* Does the exact same as above, but with a default resolution, + so backwards compatibilty is maintained. + The Resolution is kept at the default FREENECT_RESOLUTION_MEDIUM +*/ -int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freenect_depth_format fmt); +int freenect_sync_get_depth_with_res(void **depth, uint32_t *timestamp, int index, + freenect_resolution res, freenect_depth_format fmt); /* Synchronous depth function, starts the runloop if it isn't running The returned buffer is valid until this function is called again, after which the buffer must not @@ -60,12 +68,19 @@ int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freene depth: Populated with a pointer to a depth buffer with a size of the requested type timestamp: Populated with the associated timestamp index: Device index (0 is the first) + res: Valid resolution fmt: Valid format Returns: Nonzero on error. */ +int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freenect_depth_format fmt); +/* Again, a wrapper function to keep backward compatibility. + The Resolution is kept at the default FREENECT_RESOLUTION_MEDIUM + +*/ + int freenect_sync_set_tilt_degs(int angle, int index); /* Tilt function, starts the runloop if it isn't running