example.lisp

   1 ;; $ LD_PRELOAD=/usr/lib64/libv4l/v4l2convert.so sbcl --load example.lisp
   2
   3 (asdf:oos 'asdf:load-op :cl-v4l2)
   4 (asdf:oos 'asdf:load-op :cl-gtk2-gtkglext)
   5 (asdf:oos 'asdf:load-op :bordeaux-threads)
   6
   7 (defpackage :test-v4l2
   8   (:use :common-lisp :gtk :gtkglext)
   9   (:import-from :iolib.syscalls %sys-open %sys-close o-rdwr))
  10
  11 (in-package :test-v4l2)
  12
  13 (defvar *capture-device* "/dev/video0")
  14 (defparameter *capture-fd* nil)
  15
  16 ;; what we want from camera
  17 (defvar *want-width* 352)
  18 (defvar *want-height* 288)
  19
  20 ;; what we really get
  21 (defparameter *got-width* nil)
  22 (defparameter *got-height* nil)
  23
  24 (defparameter *camera-widget* nil)
  25 (defparameter *camera-data* nil)
  26 (defparameter *camera-data-lock* (bt:make-lock "Camera data lock"))
  27
  28 (defparameter *cap-thread-stop* nil)
  29
  30 (defparameter *render-thread-stop* (bt:make-condition-variable))
  31 (defparameter *render-thread-lock* (bt:make-lock "Render thread lock"))
  32
  33 (defmacro without-errors (&body body)
  34   `(handler-case (progn ,@body)
  35      (error (c) (format t "suppressed error: ~A~%" c) nil)))
  36
  37 (defun char-at (pos data)
  38   (code-char (ldb (byte 8 (* 8 pos)) data)))
  39
  40 (defun format-string (pixfmt)
  41   (format nil "~C~C~C~C"
  42           (char-at 0 pixfmt)
  43           (char-at 1 pixfmt)
  44           (char-at 2 pixfmt)
  45           (char-at 3 pixfmt)))
  46
  47 (defun diagnose (fd)
  48   (let ((caps (v4l2:query-capabilities fd)))
  49     (format t (v4l2:%device-info caps))
  50     (unless (v4l2:capable caps v4l2:cap-video-capture)
  51       (error "not a capture device"))
  52     (unless (v4l2:capable caps v4l2:cap-streaming)
  53       (error "not a streaming device"))
  54     (when (v4l2:capable caps v4l2:cap-tuner)
  55       (without-errors
  56           (loop for idx from 0 do
  57                (progn
  58                  (v4l2:get-tuner-params fd idx)
  59                  ;; show tuner params
  60                  ))))
  61
  62     (without-errors
  63         (loop for idx from 0 do
  64              (with-slots (v4l2:index v4l2:name v4l2:type v4l2:tuner)
  65                  (v4l2:get-input-params fd idx)
  66                (format t "input [~D] name: ~A, type ~A~%"
  67                        v4l2:index
  68                        v4l2:name
  69                        (if (= v4l2:type v4l2:input-type-tuner) "tuner" "camera"))
  70                (when (= v4l2:type v4l2:input-type-tuner)
  71                  (format t "input [~D] connected to tuner ~D~%" v4l2:index v4l2:tuner))
  72
  73                (without-errors
  74                    (loop for idx1 from 0 do
  75                         (with-slots (v4l2:index v4l2:name)
  76                             (v4l2:get-input-standard fd idx1)
  77                           (format t "input [~D] std [~D] name: ~A~%"
  78                                   idx v4l2:index v4l2:name)))))))
  79
  80     (v4l2:set-input fd 0)               ; some cameras don't set input by default
  81
  82     (without-errors
  83         (loop for idx from 0 do
  84              (with-slots (v4l2:index v4l2:pixelformat) (v4l2:get-format fd idx)
  85                (format t "format [~D] ~S~%" v4l2:index
  86                        (format-string v4l2:pixelformat)))))))
  87
  88 (defun device-init (fd)
  89   (v4l2:set-input fd 0)
  90   (without-errors
  91       (v4l2:set-control fd v4l2:cid-exposure 0.05))
  92   (format t "set ~Dx~D, format ~S~%" *want-width* *want-height*
  93           (format-string v4l2:pix-fmt-rgb24))
  94   (v4l2:set-image-format fd *want-width* *want-height* v4l2:pix-fmt-rgb24)
  95   (with-slots (v4l2:width v4l2:height v4l2:sizeimage v4l2:pixelformat)
  96       (v4l2:format-pix (v4l2:get-image-format fd))
  97     (setf *got-width* v4l2:width
  98           *got-height* v4l2:height)
  99     (format t "got ~Dx~D size ~D, format ~S~%"
 100             v4l2:width v4l2:height
 101             v4l2:sizeimage (format-string v4l2:pixelformat))
 102     (setq *camera-data* (make-array (* 4 *got-height* *got-width*)
 103                                     :element-type '(unsigned-byte 8)
 104                                     :initial-element #xff))))
 105
 106 (defun video-init (device)
 107   (let ((fd (%sys-open device o-rdwr)))
 108     (setq *capture-fd* fd)
 109     (diagnose fd)                                       ; info about device
 110     (device-init fd)                                    ; setup
 111     (let ((buffers (v4l2:map-buffers fd 4)))            ; map 4 buffers into memory
 112       (v4l2:stream-on fd buffers)                       ; start capturing
 113       (values fd buffers))))
 114
 115 (defun video-uninit (fd buffers)
 116   (v4l2:stream-off fd)                  ; stop capturing
 117   (v4l2:unmap-buffers buffers)          ; throw away buffers from memory
 118   (%sys-close fd)                               ; close device
 119   (format t "that's all!~%"))
 120
 121 (defun capture-thread ()
 122   (format t "cap thread start~%")
 123   (multiple-value-bind (fd buffers)
 124       (video-init *capture-device*)
 125     (loop thereis *cap-thread-stop* do
 126          (let ((frame (without-errors (v4l2:get-frame fd))))            ; get one frame from driver
 127            (when frame                  ; errors from v4l2convert.so are highly possible
 128              (multiple-value-bind (buffer address)
 129                  (values-list (nth frame buffers))
 130                (declare (ignore buffer))
 131                ;; Silly rgb24->rgb32 converter
 132                (bt:with-lock-held (*camera-data-lock*)
 133                  (declare (optimize (speed 3) (debug 0) (safety 0)))
 134                  (loop for i fixnum from 0 below (* *got-width* *got-height*) do
 135                       (let ((r (cffi:mem-aref address :uchar (+ (* 3 i) 0)))
 136                             (g (cffi:mem-aref address :uchar (+ (* 3 i) 1)))
 137                             (b (cffi:mem-aref address :uchar (+ (* 3 i) 2))))
 138
 139                         (setf (aref *camera-data* (+ (* 4 i) 0)) r
 140                               (aref *camera-data* (+ (* 4 i) 1)) g
 141                               (aref *camera-data* (+ (* 4 i) 2)) b)))))
 142
 143              (when *camera-widget*
 144                (with-main-loop
 145                    (widget-queue-draw *camera-widget*)))
 146
 147              (v4l2:put-frame fd frame))))       ; put frame back to driver
 148     (video-uninit fd buffers))
 149   (format t "cap thread exit~%"))
 150
 151 (defun camera-init (widget)
 152   (declare (ignore widget))
 153   (gl:clear-color 0.8 0.8 0.8 0.8)
 154   (gl:enable :texture-rectangle-arb :depth-test)
 155   (gl:depth-func :lequal)
 156
 157   (gl:bind-texture :texture-rectangle-arb 0)
 158
 159   (gl:tex-image-2d :texture-rectangle-arb
 160                    0
 161                    :rgb8
 162                    *got-width*
 163                    *got-height*
 164                    0
 165                    :rgba
 166                    :unsigned-byte
 167                    *camera-data*)
 168
 169   (gl:new-list 1 :compile)
 170
 171   (gl:begin :quads)
 172   (gl:tex-coord 0 *got-height*)
 173   (gl:vertex 0.0 0.0)
 174   (gl:tex-coord 0 0)
 175   (gl:vertex 0.0 1.0)
 176   (gl:tex-coord *got-width* 0)
 177   (gl:vertex 1.0 1.0)
 178   (gl:tex-coord *got-width* *got-height*)
 179   (gl:vertex 1.0 0.0)
 180   (gl:end)
 181   (gl:end-list)
 182
 183   (gl:clear-depth 1.0)
 184   (gl:flush))
 185
 186 (defun camera-draw (widget event)
 187   (declare (ignore widget event))
 188   (gl:clear :color-buffer-bit :depth-buffer-bit)
 189   (gl:bind-texture :texture-rectangle-arb 0)
 190
 191   (when *camera-data*
 192     (bt:with-lock-held (*camera-data-lock*)
 193       (gl:tex-sub-image-2d :texture-rectangle-arb 0
 194                            0 0
 195                            *got-width*
 196                            *got-height*
 197                            :rgba
 198                            :unsigned-byte
 199                            *camera-data*)))
 200
 201   ;; Keep ratio 4:3
 202   (multiple-value-bind (w h)
 203       (gdk:drawable-get-size (widget-window widget))
 204     (let ((w1 w)
 205           (h1 h))
 206       (when (and (> w 0) (> h 0))
 207         (if (> (/ w h) 4/3)
 208             (setq h1 h
 209                   w1 (* h 4/3))
 210             (setq w1 w
 211                   h1 (* w 3/4))))
 212       (gl:viewport 0 0 w1 h1)))
 213
 214   (gl:matrix-mode :projection)
 215   (gl:load-identity)
 216   (glu:perspective 19.0 1.0 1.0 10.0)
 217
 218   (gl:matrix-mode :modelview)
 219   (gl:load-identity)
 220   (glu:look-at 0.0 0.0 3.0
 221                0.0 0.0 0.0
 222                0.0 1.0 0.0)
 223
 224   (gl:translate -0.5 -0.5 0.0)
 225
 226   (gl:call-list 1)
 227   (gl:flush))
 228
 229 (defun test ()
 230   (let ((cap-thread (bt:make-thread #'capture-thread :name "capturer")))
 231     (with-main-loop
 232       (let ((window (make-instance 'gtk-window
 233                                    :type :toplevel
 234                                    :window-position :center
 235                                    :title "Hello world!"
 236                                    :default-width *want-width*
 237                                    :default-height *want-height*))
 238             (hbox (make-instance 'h-box))
 239             (vbox (make-instance 'v-box))
 240             (quit-button (make-instance 'button :label "Quit"))
 241             (bright-spin (make-instance 'spin-button :label "Brightness"
 242                                         :adjustment (make-instance 'adjustment
 243                                                                    :lower 0d0
 244                                                                    :upper 100d0
 245                                                                    :step-increment 1d0))))
 246
 247
 248         (gobject:connect-signal bright-spin "value-changed"
 249                                 (lambda (widget)
 250                                   (let ((value (adjustment-value (spin-button-adjustment bright-spin))))
 251                                     (format t "~A changed value to ~F~%" widget value)
 252                                     (unless (without-errors
 253                                                 (format t "Previous value was: ~F~%"
 254                                                         (v4l2:get-control *capture-fd* v4l2:cid-brightness))
 255                                               (v4l2:set-control *capture-fd* v4l2:cid-brightness (/ value 100)))
 256                                       (v4l2:set-control *capture-fd* v4l2:cid-exposure (/ value 100))))))
 257
 258         (gobject:connect-signal quit-button "clicked"
 259                                 (lambda (widget)
 260                                   (declare (ignore widget))
 261                                   (bt:condition-notify *render-thread-stop*)))
 262
 263         (gobject:connect-signal window "destroy"
 264                                 (lambda (widget)
 265                                   (declare (ignore widget))
 266                                   (bt:condition-notify *render-thread-stop*)))
 267
 268 ;; Capture process needs to know which widget to ask for redraw
 269         (setq *camera-widget* (make-instance 'gl-drawing-area
 270                                              :on-init #'camera-init
 271                                              :on-expose #'camera-draw))
 272         (box-pack-start hbox vbox :expand nil)
 273         (box-pack-start hbox *camera-widget* :expand t)
 274         (box-pack-start vbox quit-button :expand nil)
 275         (box-pack-start vbox bright-spin :expand nil)
 276         (container-add window hbox)
 277         (widget-show window :all t)))
 278
 279 ;; Wait for window destruction
 280     (bt:with-lock-held (*render-thread-lock*)
 281       (bt:condition-wait *render-thread-stop* *render-thread-lock*))
 282     (setq *cap-thread-stop* t)
 283     (bt:join-thread cap-thread)))
 284
 285 (test)