example.lisp

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