Port snapshot tool from IMX
Change-Id: Ibf0ebb92d1f51e79c14bd624ec7bae37b3258c77
diff --git a/debian/excelsior-tools.install b/debian/excelsior-tools.install
index 6422483..842f46e 100644
--- a/debian/excelsior-tools.install
+++ b/debian/excelsior-tools.install
@@ -1 +1,2 @@
pinout /usr/bin
+snapshot /usr/bin
diff --git a/snapshot b/snapshot
new file mode 100755
index 0000000..0aac41d
--- /dev/null
+++ b/snapshot
@@ -0,0 +1,289 @@
+#!/usr/bin/env python3
+
+import argparse
+import contextlib
+import fcntl
+import os
+import select
+import sys
+import termios
+import time
+import threading
+
+import gi
+gi.require_version('Gst', '1.0')
+gi.require_version('GstBase', '1.0')
+
+from functools import partial
+from gi.repository import GLib, GObject, Gst, GstBase
+from PIL import Image
+
+Gst.init(None)
+
+FILENAME_PREFIX = 'img'
+FILENAME_SUFFIX = '.png'
+AF_SYSFS_NODE = '/sys/module/ov5645_camera_mipi_v2/parameters/ov5645_af'
+CAMERA_INIT_QUERY_SYSFS_NODE = '/sys/module/ov5645_camera_mipi_v2/parameters/ov5645_initialized'
+HDMI_SYSFS_NODE = '/sys/class/drm/card0/card0-HDMI-A-1/status'
+
+# No of initial frames to throw away before camera has stabilized
+SCRAP_FRAMES = 1
+
+SRC_WIDTH = 2592
+SRC_HEIGHT = 1944
+SRC_RATE = '15/1'
+SRC_ELEMENT = 'v4l2src device=/dev/video0'
+
+SINK_WIDTH = 2592
+SINK_HEIGHT = 1944
+SINK_ELEMENT = ('appsink name=appsink sync=false emit-signals=true '
+ 'max-buffers=1 drop=true')
+SCREEN_SINK = 'glimagesink sync=false'
+FAKE_SINK = 'fakesink sync=false'
+
+SRC_CAPS = 'video/x-raw,format=YUY2,width={width},height={height},framerate={rate}'
+SINK_CAPS = 'video/x-raw,format=RGB,width={width},height={height}'
+LEAKY_Q = 'queue max-size-buffers=1 leaky=downstream'
+
+PIPELINE = '''
+ {src_element} ! {src_caps} ! {leaky_q} ! tee name=t
+ t. ! {leaky_q} ! {screen_sink}
+ t. ! {leaky_q} ! videoconvert ! {sink_caps} ! {sink_element}
+ '''
+
+def monitor_connected():
+ with open(HDMI_SYSFS_NODE, 'r') as hdmi_status:
+ status = hdmi_status.read()
+ return (status.rstrip() == 'connected')
+
+def on_bus_message(bus, message, loop):
+ t = message.type
+ if t == Gst.MessageType.EOS:
+ loop.quit()
+ elif t == Gst.MessageType.WARNING:
+ err, debug = message.parse_warning()
+ sys.stderr.write('Warning: %s: %s\n' % (err, debug))
+ elif t == Gst.MessageType.ERROR:
+ err, debug = message.parse_error()
+ sys.stderr.write('Error: %s: %s\n' % (err, debug))
+ loop.quit()
+ return True
+
+
+def on_new_sample(sink, snapinfo):
+
+ if not snapinfo.save_frame():
+ # Throw away the frame
+ return Gst.FlowReturn.OK
+
+ sample = sink.emit('pull-sample')
+
+ buf = sample.get_buffer()
+ result, mapinfo = buf.map(Gst.MapFlags.READ)
+ if result:
+ imgfile = snapinfo.get_filename()
+ print('Saving image: ' + imgfile)
+ caps = sample.get_caps()
+ width = caps.get_structure(0).get_value('width')
+ height = caps.get_structure(0).get_value('height')
+ img = Image.frombytes('RGB', (width, height), mapinfo.data, 'raw')
+ img.save(imgfile)
+ img.close()
+ buf.unmap(mapinfo)
+ return Gst.FlowReturn.OK
+
+
+def run_pipeline(snapinfo):
+ src_caps = SRC_CAPS.format(width=SRC_WIDTH, height=SRC_HEIGHT, rate=SRC_RATE)
+ sink_caps = SINK_CAPS.format(width=SINK_WIDTH, height=SINK_HEIGHT)
+
+ if snapinfo.oneshot or not monitor_connected():
+ screen_sink = FAKE_SINK
+ else:
+ screen_sink = SCREEN_SINK
+
+ pipeline = PIPELINE.format(
+ leaky_q=LEAKY_Q,
+ src_element=SRC_ELEMENT,
+ src_caps=src_caps,
+ sink_caps=sink_caps,
+ sink_element=SINK_ELEMENT,
+ screen_sink=screen_sink)
+
+ pipeline = Gst.parse_launch(pipeline)
+ appsink = pipeline.get_by_name('appsink')
+ appsink.connect('new-sample', partial(on_new_sample, snapinfo=snapinfo))
+
+ loop = GLib.MainLoop()
+
+ # Set up a pipeline bus watch to catch errors.
+ bus = pipeline.get_bus()
+ bus.add_signal_watch()
+ bus.connect('message', on_bus_message, loop)
+
+ # Connect the loop to the snaphelper
+ snapinfo.connect_loop(loop)
+
+ # Run pipeline.
+ pipeline.set_state(Gst.State.PLAYING)
+
+ try:
+ loop.run()
+ except:
+ pass
+
+ # Clean up.
+ pipeline.set_state(Gst.State.NULL)
+ while GLib.MainContext.default().iteration(False):
+ pass
+
+
+@contextlib.contextmanager
+def setup_keyboard():
+ fd = sys.stdin.fileno()
+ termattr = termios.tcgetattr(fd)
+ orgattr = list(termattr)
+ termattr[3] = termattr[3] & ~(termios.ICANON | termios.ECHO)
+ termios.tcsetattr(fd, termios.TCSANOW, termattr)
+ orgflags = fcntl.fcntl(fd, fcntl.F_GETFL)
+ fcntl.fcntl(fd, fcntl.F_SETFL, orgflags | os.O_NONBLOCK)
+
+ try:
+ yield
+ finally:
+ termios.tcsetattr(fd, termios.TCSAFLUSH, orgattr)
+ fcntl.fcntl(fd, fcntl.F_SETFL, orgflags)
+
+class SnapHelper:
+
+ def __init__(self, sysfs, prefix='img', oneshot=True, suffix='jpg'):
+ self.prefix = prefix
+ self.oneshot = oneshot
+ self.suffix = suffix
+ self.snap_it = oneshot
+ self.num = 0
+ self.scrapframes = SCRAP_FRAMES
+ self.sysfs = sysfs
+ self.loop = None
+
+ if not oneshot:
+ self.pipe_r, self.pipe_w = os.pipe()
+ self.thread = threading.Thread(target=self.read_keyboard)
+ self.thread.daemon = True
+
+ def get_filename(self):
+ while True:
+ filename = self.prefix + str(self.num).zfill(4) + '.' + self.suffix
+ self.num = self.num + 1
+ if not os.path.exists(filename):
+ break
+
+ return filename
+
+ def read_keyboard(self):
+ print('Press space to take a snap, r to refocus, or q to quit')
+ with setup_keyboard():
+ while True:
+ read_fd, _, _ = select.select([sys.stdin, self.pipe_r], [], [])
+ if self.pipe_r in read_fd:
+ break
+ c = sys.stdin.read()
+ if c == ' ':
+ self.snap_it = True
+ if c == 'r':
+ self.refocus()
+ if c == 'q':
+ while not self.loop.is_running():
+ time.sleep(0.01)
+ self.loop.quit()
+ break
+
+ def exit_keyboard_thread(self):
+ try:
+ os.close(self.pipe_w)
+ except:
+ pass
+
+ def check_af(self):
+ try:
+ self.sysfs.seek(0)
+ v = self.sysfs.read()
+ if int(v) != 0x10:
+ print('NO Focus')
+ except:
+ pass
+
+ def refocus(self):
+ try:
+ self.sysfs.write('1')
+ self.sysfs.flush()
+ except:
+ pass
+
+ def save_frame(self):
+ # We always want to throw away the initial frames to let the
+ # camera stabilize. This seemed empirically to be the right number
+ # when running on desktop.
+ if self.scrapframes > 0:
+ if self.scrapframes == SCRAP_FRAMES:
+ self.refocus()
+ self.scrapframes = self.scrapframes - 1
+ return False
+
+ if self.snap_it:
+ self.check_af()
+ self.snap_it = False
+ retval = True
+ else:
+ retval = False
+
+ if self.oneshot:
+ self.loop.quit()
+
+ return retval
+
+ def connect_loop(self, loop):
+ self.loop = loop
+ self.thread.start()
+
+
+def main(arguments):
+ parser = argparse.ArgumentParser('Take camera snapshots')
+ parser.add_argument(
+ '--prefix',
+ '-p',
+ dest='prefix',
+ help='Filename prefix',
+ default=FILENAME_PREFIX)
+ parser.add_argument(
+ '--oneshot',
+ dest='oneshot',
+ action='store_true',
+ help='One shot vs. interactive mode')
+ parser.add_argument(
+ '--format',
+ '-f',
+ dest='suffix',
+ default='jpg',
+ choices=('jpg', 'bmp', 'png'),
+ help='Format to save, default is JPEG')
+ args = parser.parse_args()
+
+ try:
+ with open(CAMERA_INIT_QUERY_SYSFS_NODE) as init_file:
+ init_file.seek(0)
+ init = init_file.read()
+ if int(init) != 1:
+ raise Exception('Cannot find ov5645 CSI camera, ' +
+ 'check that your camera is connected')
+ with open(AF_SYSFS_NODE, 'w+') as sysfs:
+ snap = SnapHelper(sysfs, args.prefix, args.oneshot, args.suffix)
+ run_pipeline(snap)
+ snap.exit_keyboard_thread()
+ except Exception as ex:
+ print(ex)
+
+
+if __name__ == '__main__':
+ main(sys.argv)
+ sys.exit()
