// Custom camera initialiazation function
void custom_v4l2_init(void* parm_void)
{
#define V4L2_CID_C920_ZOOMVAL 0x9A090D //Zoom (wide=0, telephoto=500)
#define V4L2_CID_C920_AUTOFOCUS 0x9A090C //Autofocus (0=OFF, 1 = ON)
#define V4L2_CID_C920_FOCUSVAL 0X9A090A //Focus Value (min=0, max=250)
#define V4L2_C920_FOCUS_INF 0
#define V4L2_C920_FOCUS_MACRO 250
struct v4l2Parms* parm = (struct v4l2Parms*) parm_void;
set_parm(parm->fd, V4L2_CID_C920_AUTOFOCUS,0); // Turn autofocus off
set_parm(parm->fd, V4L2_CID_C920_FOCUSVAL,V4L2_C920_FOCUS_INF); // Use infinity focus (no macro)
set_parm(parm->fd, V4L2_CID_C920_ZOOMVAL,107); // Zoom (trying to match PS3 Eye)
//set_parm(parm->fd, V4L2_CID_C920_ZOOMVAL,150);
set_parm(parm->fd, V4L2_CID_SATURATION,170); // Adjust Saturation (0-255)
set_parm(parm->fd, V4L2_CID_SHARPNESS,128); // Blur the image to get smoother contours (0-255)
#ifdef INDOOR
set_manual_exposure(parm->fd, 20); // Indoor exposure
set_parm(parm->fd, V4L2_CID_GAIN, 255); // Indoor gain
#endif
#ifdef OUTDOOR
set_manual_exposure(parm->fd, 3); // Outdoor exposure (4 to 2046)
set_parm(parm->fd, V4L2_CID_GAIN, 15); // Outdoor gain
#endif
#ifdef AUTO
set_auto_exposure(parm->fd);
#endif
set_parm(parm->fd, V4L2_CID_BRIGHTNESS, 128);
set_parm(parm->fd, V4L2_CID_CONTRAST, 128);
}
void camera_server() {
size_t count = 0;
initialise_termination_handler();
int deviceDescriptor = v4l2_open("/dev/video0",
O_RDWR /* required */| O_NONBLOCK, 0);
if (deviceDescriptor == -1) {
throw std::runtime_error("Unable to open device");
}
// disable_output_processing(deviceDescriptor);
if (!isStreamingIOSupported(deviceDescriptor)) {
throw std::runtime_error("Streaming is not supported");
}
setCameraOutputFormat(deviceDescriptor, CAMERA_FRAME_WIDTH, CAMERA_FRAME_HEIGHT, V4L2_PIX_FMT_YUYV);
std::cout << "Absolute focus supported: " << isControlSupported(deviceDescriptor,V4L2_CID_FOCUS_ABSOLUTE) << std::endl;
std::cout << "Relative focus supported: " << isControlSupported(deviceDescriptor,V4L2_CID_FOCUS_RELATIVE) << std::endl;
set_manual_exposure(deviceDescriptor,true);
printf("Is manual exposure set = %u\n", is_manual_exposure(deviceDescriptor));
set_absolute_exposure(100,deviceDescriptor);
set_exposure_auto_priority(deviceDescriptor,false);
printf("Is exposure auto priority set = %u\n", is_exposure_auto_priority(deviceDescriptor));
set_auto_white_balance(deviceDescriptor,false);
printf("Is auto white balance set = %u\n", is_auto_white_balance_set(deviceDescriptor));
set_gain(deviceDescriptor,1);
printf("Gain set = %u\n", get_gain(deviceDescriptor));
printf("Focus value = %u\n", get_focus_variable(deviceDescriptor));
set_fps(deviceDescriptor,30);
start_capturing(deviceDescriptor);
unsigned int counter;
int announce_socket=socket(AF_INET,SOCK_DGRAM,0);
if (announce_socket < 0) {
perror("socket");
exit(1);
}
sockaddr_in announce_address;
memset(&announce_address,0,sizeof(announce_address));
announce_address.sin_family=AF_INET;
announce_address.sin_addr.s_addr=inet_addr(CAMERA_ANNOUNCE_GROUP);
announce_address.sin_port=htons(CAMERA_ANNOUNCE_PORT);
while (running != 0) {
fd_set fds;
int r;
FD_ZERO(&fds);
FD_SET(deviceDescriptor, &fds);
r = select(deviceDescriptor + 1, &fds, NULL, NULL, NULL);
if (r > 0) {
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl(deviceDescriptor, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
continue;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
perror("VIDIOC_DQBUF");
exit(1);
}
}
if ((buf.flags | V4L2_BUF_FLAG_ERROR) != 0) {
//TODO Investigate the permanent occurence of the V4L2_BUF_FLAG_ERROR
// std::cerr << "Frame buffer error" << std::endl;
}
printf("Index = %u, seconds = %ld us = %ld\n", buf.index,buf.timestamp.tv_sec,buf.timestamp.tv_usec);
// printf("Real time: seconds = %ld, us = %ld\n", tp.tv_sec,tp.tv_nsec/1000);
int ret;
assert(ptrToSequenceMap.count(buf.m.userptr) != 0);
size_t sequence_number = ptrToSequenceMap[buf.m.userptr];
ptrToSequenceMap.erase(buf.m.userptr);
queueNextFrameBuffer(deviceDescriptor, buf.index, sequence_number, CAMERA_FRAME_WIDTH*CAMERA_FRAME_HEIGHT*2);
//TODO Investigate why the video streaming fails if the unmap call below is placed before the queueNextFrameBuffer call above.
//Probably this is because in that case the mmap call returns the same virtual address as the munmap call had just used for the deallocation
ret = munmap(reinterpret_cast<void*>(buf.m.userptr),buf.length);
if (ret == -1) {
perror("munmap");
}
BufferReference readyBuffer;
readyBuffer.index = buf.index;
readyBuffer.offset = 0;
readyBuffer.size = buf.bytesused;
readyBuffer.timestamp_seconds = buf.timestamp.tv_sec;
readyBuffer.timestamp_microseconds = buf.timestamp.tv_usec;
readyBuffer.width = CAMERA_FRAME_WIDTH;
readyBuffer.height = CAMERA_FRAME_HEIGHT;
readyBuffer.sequence = sequence_number;
std::array<char,1024> ipc_buffer;
asn_enc_rval_t encode_result = der_encode_to_buffer(&asn_DEF_BufferReference, &readyBuffer,ipc_buffer.data(),ipc_buffer.size());
ret = sendto(announce_socket,ipc_buffer.data(),encode_result.encoded,0,(struct sockaddr *) &announce_address,sizeof(announce_address));
if (ret < 0) {
perror("sendto");
exit(1);
}
timespec tp;
clock_gettime(CLOCK_MONOTONIC,&tp);
std::cout << "Grab frame delay = " << get_milliseconds_delta(buf.timestamp,tp) << " ms" << std::endl;
count++;
counter++;
}
}
v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(deviceDescriptor, VIDIOC_STREAMOFF, &type))
perror("VIDIOC_STREAMOFF");
if (-1 == close(deviceDescriptor))
perror("close");
close(announce_socket);
}