Skip to content

Commit 334c1ee

Browse files
marcanqdot
marcan
authored and
qdot
committed
Support IR video stream
Signed-off-by: Hector Martin <hector@marcansoft.com>
1 parent 9c2f4f4 commit 334c1ee

File tree

3 files changed

+80
-13
lines changed

3 files changed

+80
-13
lines changed

‎include/libfreenect.h

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,15 @@ extern "C" {
3737
#define FREENECT_FRAME_H 480
3838
#define FREENECT_FRAME_PIX (FREENECT_FRAME_H*FREENECT_FRAME_W)
3939

40+
#define FREENECT_IR_FRAME_W 640
41+
#define FREENECT_IR_FRAME_H 488
42+
#define FREENECT_IR_FRAME_PIX (FREENECT_IR_FRAME_H*FREENECT_IR_FRAME_W)
43+
4044
#define FREENECT_VIDEO_RGB_SIZE (FREENECT_FRAME_PIX*3)
4145
#define FREENECT_VIDEO_BAYER_SIZE (FREENECT_FRAME_PIX)
46+
#define FREENECT_VIDEO_IR_8BIT_SIZE (FREENECT_IR_FRAME_PIX)
47+
#define FREENECT_VIDEO_IR_10BIT_SIZE (FREENECT_IR_FRAME_PIX*sizeof(uint16_t))
48+
#define FREENECT_VIDEO_IR_10BIT_PACKED_SIZE 390400
4249

4350
#define FREENECT_DEPTH_11BIT_SIZE (FREENECT_FRAME_PIX*sizeof(uint16_t))
4451
#define FREENECT_DEPTH_10BIT_SIZE FREENECT_DEPTH_11BIT_SIZE
@@ -50,6 +57,9 @@ extern "C" {
5057
typedef enum {
5158
FREENECT_VIDEO_RGB = 0,
5259
FREENECT_VIDEO_BAYER = 1,
60+
FREENECT_VIDEO_IR_8BIT = 2,
61+
FREENECT_VIDEO_IR_10BIT = 3,
62+
FREENECT_VIDEO_IR_10BIT_PACKED = 4,
5363
} freenect_video_format;
5464

5565
typedef enum {

‎src/cameras.c

Lines changed: 69 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -204,13 +204,12 @@ static int stream_setbuf(freenect_context *ctx, packet_stream *strm, void *pbuf)
204204
}
205205

206206
// Unpack buffer of (vw bit) data into padded 16bit buffer.
207-
static inline void convert_packed_to_16bit(uint8_t *raw, uint16_t *frame, int vw)
207+
static inline void convert_packed_to_16bit(uint8_t *raw, uint16_t *frame, int vw, int len)
208208
{
209209
int mask = (1 << vw) - 1;
210-
int j = 640*480;
211210
uint32_t buffer = 0;
212211
int bitsIn = 0;
213-
while (j--) {
212+
while (len--) {
214213
while (bitsIn < vw) {
215214
buffer = (buffer << 8) | *(raw++);
216215
bitsIn += 8;
@@ -220,6 +219,21 @@ static inline void convert_packed_to_16bit(uint8_t *raw, uint16_t *frame, int vw
220219
}
221220
}
222221

222+
// Unpack buffer of (vw bit) data into 8bit buffer, dropping LSBs
223+
static inline void convert_packed_to_8bit(uint8_t *raw, uint8_t *frame, int vw, int len)
224+
{
225+
uint32_t buffer = 0;
226+
int bitsIn = 0;
227+
while (len--) {
228+
while (bitsIn < vw) {
229+
buffer = (buffer << 8) | *(raw++);
230+
bitsIn += 8;
231+
}
232+
bitsIn -= vw;
233+
*(frame++) = buffer >> (bitsIn+vw-8);
234+
}
235+
}
236+
223237
static void depth_process(freenect_device *dev, uint8_t *pkt, int len)
224238
{
225239
freenect_context *ctx = dev->parent;
@@ -240,10 +254,10 @@ static void depth_process(freenect_device *dev, uint8_t *pkt, int len)
240254

241255
switch (dev->depth_format) {
242256
case FREENECT_DEPTH_11BIT:
243-
convert_packed_to_16bit(dev->depth.raw_buf, dev->depth.proc_buf, 11);
257+
convert_packed_to_16bit(dev->depth.raw_buf, dev->depth.proc_buf, 11, FREENECT_FRAME_PIX);
244258
break;
245259
case FREENECT_DEPTH_10BIT:
246-
convert_packed_to_16bit(dev->depth.raw_buf, dev->depth.proc_buf, 10);
260+
convert_packed_to_16bit(dev->depth.raw_buf, dev->depth.proc_buf, 10, FREENECT_FRAME_PIX);
247261
break;
248262
case FREENECT_DEPTH_10BIT_PACKED:
249263
case FREENECT_DEPTH_11BIT_PACKED:
@@ -435,8 +449,20 @@ static void video_process(freenect_device *dev, uint8_t *pkt, int len)
435449
FN_SPEW("Got RGB frame %d/%d packets arrived, TS %08x\n", dev->video.valid_pkts,
436450
dev->video.pkts_per_frame, dev->video.timestamp);
437451

438-
if (dev->video_format == FREENECT_VIDEO_RGB) {
439-
convert_bayer_to_rgb(dev->video.raw_buf, dev->video.proc_buf);
452+
switch (dev->video_format) {
453+
case FREENECT_VIDEO_RGB:
454+
convert_bayer_to_rgb(dev->video.raw_buf, dev->video.proc_buf);
455+
break;
456+
case FREENECT_VIDEO_BAYER:
457+
break;
458+
case FREENECT_VIDEO_IR_10BIT:
459+
convert_packed_to_16bit(dev->video.raw_buf, dev->video.proc_buf, 10, FREENECT_IR_FRAME_PIX);
460+
break;
461+
case FREENECT_VIDEO_IR_10BIT_PACKED:
462+
break;
463+
case FREENECT_VIDEO_IR_8BIT:
464+
convert_packed_to_8bit(dev->video.raw_buf, dev->video.proc_buf, 10, FREENECT_IR_FRAME_PIX);
465+
break;
440466
}
441467

442468
if (dev->video_cb)
@@ -601,12 +627,29 @@ int freenect_start_video(freenect_device *dev)
601627
if (dev->video.running)
602628
return -1;
603629

604-
if (dev->video_format == FREENECT_VIDEO_RGB)
605-
stream_initbufs(ctx, &dev->video, FREENECT_VIDEO_BAYER_SIZE, FREENECT_VIDEO_RGB_SIZE);
606-
else
607-
stream_initbufs(ctx, &dev->video, 0, FREENECT_VIDEO_BAYER_SIZE);
630+
switch (dev->video_format) {
631+
case FREENECT_VIDEO_RGB:
632+
stream_initbufs(ctx, &dev->video, FREENECT_VIDEO_BAYER_SIZE, FREENECT_VIDEO_RGB_SIZE);
633+
dev->video.pkts_per_frame = VIDEO_PKTS_PER_FRAME;
634+
break;
635+
case FREENECT_VIDEO_BAYER:
636+
stream_initbufs(ctx, &dev->video, 0, FREENECT_VIDEO_BAYER_SIZE);
637+
dev->video.pkts_per_frame = VIDEO_PKTS_PER_FRAME;
638+
break;
639+
case FREENECT_VIDEO_IR_8BIT:
640+
stream_initbufs(ctx, &dev->video, FREENECT_VIDEO_IR_10BIT_PACKED_SIZE, FREENECT_VIDEO_IR_8BIT_SIZE);
641+
dev->video.pkts_per_frame = VIDEO_PKTS_PER_FRAME_IR;
642+
break;
643+
case FREENECT_VIDEO_IR_10BIT:
644+
stream_initbufs(ctx, &dev->video, FREENECT_VIDEO_IR_10BIT_PACKED_SIZE, FREENECT_VIDEO_BAYER_SIZE);
645+
dev->video.pkts_per_frame = VIDEO_PKTS_PER_FRAME_IR;
646+
break;
647+
case FREENECT_VIDEO_IR_10BIT_PACKED:
648+
stream_initbufs(ctx, &dev->video, 0, FREENECT_VIDEO_IR_10BIT_PACKED_SIZE);
649+
dev->video.pkts_per_frame = VIDEO_PKTS_PER_FRAME_IR;
650+
break;
651+
}
608652

609-
dev->video.pkts_per_frame = VIDEO_PKTS_PER_FRAME;
610653
dev->video.pkt_size = VIDEO_PKTDSIZE;
611654
dev->video.synced = 0;
612655
dev->video.flag = 0x80;
@@ -620,7 +663,17 @@ int freenect_start_video(freenect_device *dev)
620663
write_register(dev, 0x0c, 0x00);
621664
write_register(dev, 0x0d, 0x01);
622665
write_register(dev, 0x0e, 0x1e); // 30Hz bayer
623-
write_register(dev, 0x05, 0x01); // start video stream
666+
switch (dev->video_format) {
667+
case FREENECT_VIDEO_RGB:
668+
case FREENECT_VIDEO_BAYER:
669+
write_register(dev, 0x05, 0x01); // start video stream
670+
break;
671+
case FREENECT_VIDEO_IR_8BIT:
672+
case FREENECT_VIDEO_IR_10BIT:
673+
case FREENECT_VIDEO_IR_10BIT_PACKED:
674+
write_register(dev, 0x05, 0x03); // start video stream
675+
break;
676+
}
624677
write_register(dev, 0x47, 0x00); // disable Hflip
625678

626679
dev->video.running = 1;
@@ -690,6 +743,9 @@ int freenect_set_video_format(freenect_device *dev, freenect_video_format fmt)
690743
switch (fmt) {
691744
case FREENECT_VIDEO_RGB:
692745
case FREENECT_VIDEO_BAYER:
746+
case FREENECT_VIDEO_IR_8BIT:
747+
case FREENECT_VIDEO_IR_10BIT:
748+
case FREENECT_VIDEO_IR_10BIT_PACKED:
693749
dev->video_format = fmt;
694750
return 0;
695751
default:

‎src/freenect_internal.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,7 @@ static inline uint32_t fn_le32(uint32_t d)
9191
#define DEPTH_PKTS_10_BIT_PER_FRAME ((FREENECT_DEPTH_10BIT_PACKED_SIZE+DEPTH_PKTDSIZE-1)/DEPTH_PKTDSIZE)
9292
#define DEPTH_PKTS_11_BIT_PER_FRAME ((FREENECT_DEPTH_11BIT_PACKED_SIZE+DEPTH_PKTDSIZE-1)/DEPTH_PKTDSIZE)
9393
#define VIDEO_PKTS_PER_FRAME ((FRAME_PIX+VIDEO_PKTDSIZE-1)/VIDEO_PKTDSIZE)
94+
#define VIDEO_PKTS_PER_FRAME_IR ((FREENECT_VIDEO_IR_10BIT_PACKED_SIZE+VIDEO_PKTDSIZE-1)/VIDEO_PKTDSIZE)
9495

9596
#define VID_MICROSOFT 0x45e
9697
#define PID_NUI_CAMERA 0x02ae

0 commit comments

Comments
 (0)