//static bool first = true; // TODO: this is a hack for testing.
void V4L2CaptureStream::threadMain()// throws CaptureException;
{
int res;
//if (first)
{ printf("V4L2CaptureStream::threadMain()\n");
fflush(stdout);
streamThrottle.waitUntilStart();
//first = false;
printf("V4L2CaptureStream streamThrottle.waitUntilStart completed\n");
fflush(stdout);
}
res = fg2_startCapture(fg);
if (res != 0)
FailWithException("fg2_startCapture failed", res);
while (!disposing)
{
struct my_buffer* frame = NULL;
// TODO: support double-buffering.
// printf("V4L2CaptureStream::fg2_grab...\n");
//fflush(stdout);
frame = getFrameBuffer( fg );
// printf("V4L2CaptureStream::fg2_grab: %lx\n", (unsigned long) frame);
//fflush(stdout);
if (frame == 0)
FailWithException("getFrameBuffer failed", -1); // TODO: notify observer instead.
void *data = frame->start;
int width = format.getWidth();
int height = format.getHeight();
switch (formatTypeV4L2)
{
// TODO: other formats
case V4L2_PIX_FMT_RGB24:
if (observer != 0)
{
Image image = Image(format, (unsigned char *) data, width * height * 3);
observer->onNewImage(this, &image);
}
break;
case V4L2_PIX_FMT_YUYV:
if (observer != 0)
{
// 4 bytes = 2 pixels
Image image = Image(format, yuyvToRGB((unsigned char *) data, width, height), width * height * 3, true);
observer->onNewImage(this, &image);
}
break;
case V4L2_PIX_FMT_RGB32:
if (observer != 0)
{
Image image = Image(format, (unsigned char *) data, width * height * 4);
observer->onNewImage(this, &image);
}
break;
case V4L2_PIX_FMT_YUV420:
{ if (rgbbufsize == 0)
{ rgbbufsize = width * height * 3;
rgbbuf = new unsigned char[rgbbufsize];
}
yuv2rgb_buf((unsigned char *) data, width, height, rgbbuf);
if (observer != 0)
{
Image image = Image(format, rgbbuf, rgbbufsize);
observer->onNewImage(this, &image);
}
}
break;
default:
printf("unknown or unsupported: %i\n", formatTypeV4L2);
//FailWithException("unknown or unsupported format", formatTypeV4L2);
}
giveBackFrameBuffer(fg, frame);
}
res = fg2_stopCapture(fg);
if (res != 0)
FailWithException("fg2_stopCapture failed", res);
fg2_delete(&fg);
if (rgbbuf != 0)
{ delete[] rgbbuf;
rgbbuf = 0;
}
disposed = true;
}
/**
@brief Update the device data (the data going back to the client).
*/
void CameraV4L2::WriteData()
{
size_t image_size, size;
unsigned char * ptr1, * ptr2;
struct my_buffer *v4lBuffer = getFrameBuffer(fg);
if (v4lBuffer==NULL)
exit(1);
ptr1 = (unsigned char *)v4lBuffer->start;
ptr2 = this->data.image;
// Compute size of image
image_size = this->width * this->height * this->depth / 8;
// Set the image properties
this->data.width = htons(this->width);
this->data.height = htons(this->height);
this->data.bpp = this->depth;
this->data.compression = PLAYER_CAMERA_COMPRESS_RAW;
this->data.image_size = htonl(image_size);
if (image_size > sizeof(this->data.image)){
PLAYER_ERROR2("image_size <= sizeof(this->data.image) failed: %d > %d",
image_size, sizeof(this->data.image));
}
assert(image_size <= sizeof(this->data.image));
if (image_size > (size_t) v4lBuffer->length){
PLAYER_WARN("Frame size is smaller then expected");
image_size = (size_t) v4lBuffer->length;
}
//assert(image_size <= (size_t) v4lBuffer->length);
if (!flip_rb) {
memcpy(ptr2, ptr1, image_size);
} else {
int imgSize = ((this->width) * (this->height));
int i;
switch (v4l2_type_id)
{
case V4L2_PIX_FMT_RGB24:
case V4L2_PIX_FMT_BGR24:
for (i = 0; i < imgSize; i++)
{
ptr2[0] = ptr1[2];
ptr2[1] = ptr1[1];
ptr2[2] = ptr1[0];
ptr1 += 3;
ptr2 += 3;
}
break;
case V4L2_PIX_FMT_RGB32:
case V4L2_PIX_FMT_BGR32:
for (i = 0; i < imgSize; i++)
{
ptr2[0] = ptr1[2];
ptr2[1] = ptr1[1];
ptr2[2] = ptr1[0];
ptr2[3] = ptr1[3];
ptr1 += 4;
ptr2 += 4;
}
break;
default:
memcpy(ptr2, ptr1, image_size);
}
}
// Copy data to server
size = sizeof(this->data) - sizeof(this->data.image) + image_size;
struct timeval timestamp;
timestamp.tv_sec = this->tsec;
timestamp.tv_usec = this->tusec;
PutData((void*) &this->data, size, ×tamp);
giveBackFrameBuffer(fg, v4lBuffer);
return;
}
