-
Notifications
You must be signed in to change notification settings - Fork 0
/
camera.py
160 lines (138 loc) · 4.94 KB
/
camera.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
import v4l2
import fcntl
import mmap
import select
import time
from logging import getLogger
l = getLogger(__name__)
from numpy import fromstring, uint16
class Camera:
def __init__(self):
self.W = 1920
self.H = 1080
self.controls = {}
#open
l.debug("open")
self.vd = open('/dev/video0', 'rb+', buffering=0)
#queryctrl/g_ctrl
qctrl = v4l2.v4l2_queryctrl()
ctrl = v4l2.v4l2_control()
#brightness
qctrl.id = v4l2.V4L2_CID_BRIGHTNESS
try:
fcntl.ioctl(self.vd, v4l2.VIDIOC_QUERYCTRL, qctrl)
ctrl.id = qctrl.id
fcntl.ioctl(self.vd, v4l2.VIDIOC_G_CTRL, ctrl)
except:
l.error("QUERYCTRL/G_CTRL failed")
self.controls["brightness"] = (ctrl.id, ctrl.value, qctrl.minimum, qctrl.maximum)
#exposure
qctrl.id = v4l2.V4L2_CID_EXPOSURE_ABSOLUTE
try:
fcntl.ioctl(self.vd, v4l2.VIDIOC_QUERYCTRL, qctrl)
ctrl.id = qctrl.id
fcntl.ioctl(self.vd, v4l2.VIDIOC_G_CTRL, ctrl)
except:
l.error("QUERYCTRL/G_CTRL failed")
self.controls["exposure"] = (ctrl.id, ctrl.value, qctrl.minimum, qctrl.maximum)
#querycap
l.debug("querycap")
cp = v4l2.v4l2_capability()
fcntl.ioctl(self.vd, v4l2.VIDIOC_QUERYCAP, cp)
#s_fmt
l.debug("s_fmt")
fmt = v4l2.v4l2_format()
fmt.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
fmt.fmt.pix.width = self.W
fmt.fmt.pix.height = self.H
fmt.fmt.pix.pixelformat = v4l2.V4L2_PIX_FMT_SBGGR10
fmt.fmt.pix.field = v4l2.V4L2_FIELD_NONE
fcntl.ioctl(self.vd, v4l2.VIDIOC_S_FMT, fmt)
#g_fmt
fmt = v4l2.v4l2_format()
fmt.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
fcntl.ioctl(self.vd, v4l2.VIDIOC_G_FMT, fmt)
self.framesize = fmt.fmt.pix.sizeimage
#reqbufs
l.debug("reqbufs")
req = v4l2.v4l2_requestbuffers()
req.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
req.memory = v4l2.V4L2_MEMORY_MMAP
req.count = 4
fcntl.ioctl(self.vd, v4l2.VIDIOC_REQBUFS, req)
self.buffers = []
for ind in range(req.count):
#querybufs
buf = v4l2.v4l2_buffer()
buf.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
buf.memory = v4l2.V4L2_MEMORY_MMAP
buf.index = ind
fcntl.ioctl(self.vd, v4l2.VIDIOC_QUERYBUF, buf)
#mmap
mm = mmap.mmap(self.vd.fileno(), buf.length, mmap.MAP_SHARED, mmap.PROT_READ | mmap.PROT_WRITE, offset=buf.m.offset)
self.buffers.append(mm)
#qbuf
fcntl.ioctl(self.vd, v4l2.VIDIOC_QBUF, buf)
#streamon
l.debug("streamon")
buf_type = v4l2.v4l2_buf_type(v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE)
fcntl.ioctl(self.vd, v4l2.VIDIOC_STREAMON, buf_type)
#select
l.debug("select")
t0 = time.time()
max_t = 1
ready_to_read, ready_to_write, in_error = ([], [], [])
while len(ready_to_read) == 0 and time.time() - t0 < max_t:
ready_to_read, ready_to_write, in_error = select.select([self.vd], [], [], max_t)
def get_frame(self):
#dqbuf
buf = v4l2.v4l2_buffer()
l.debug("dqbuf")
buf.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
buf.memory = v4l2.V4L2_MEMORY_MMAP
fcntl.ioctl(self.vd, v4l2.VIDIOC_DQBUF, buf)
l.debug("process")
#convert to numpy array
mm = self.buffers[buf.index]
frame = mm.read(self.framesize)
img = fromstring(frame, dtype=uint16).reshape(self.H, self.W)
#save RAW
#out = open("image.raw", "wb")
#out.write(frame)
#out.close()
mm.seek(0)
#qbuf
l.debug("qbuf")
fcntl.ioctl(self.vd, v4l2.VIDIOC_QBUF, buf)
return img
def get_ctrl(self, ctrl_name):
(idx, v, mn, mx) = self.controls[ctrl_name]
ctrl = v4l2.v4l2_control()
ctrl.id = idx
try:
fcntl.ioctl(self.vd, v4l2.VIDIOC_G_CTRL, ctrl)
self.controls[ctrl_name] = (idx, ctrl.value, mn, mx)
return ctrl.value
except:
l.error("G_CTRL failed")
def set_ctrl(self, ctrl_name, val):
(idx, v, mn, mx) = self.controls[ctrl_name]
if val > mx: val = mx
if val < mn: val = mn
ctrl = v4l2.v4l2_control()
ctrl.id = idx
ctrl.value = val
try:
fcntl.ioctl(self.vd, v4l2.VIDIOC_S_CTRL, ctrl)
self.controls[ctrl_name] = (idx, ctrl.value, mn, mx)
l.debug("New controls: ", str(self.controls))
except:
l.error("S_CTRL failed")
def close(self):
#streamoff
l.debug("streamoff")
buf_type = v4l2.v4l2_buf_type(v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE)
fcntl.ioctl(self.vd, v4l2.VIDIOC_STREAMOFF, buf_type)
#close
l.debug("close")
self.vd.close()