Prototype replacement python touch driver for HyperPixel

hyperpixel-touch

#!/usr/bin/env python

import os
import signal
import sys
import time

from datetime import datetime
from threading import Timer

try:
    from evdev import uinput, UInput, AbsInfo, ecodes as e
except ImportError:
    exit("This service requires the evdev module\nInstall with: sudo pip install evdev")

try:
    import RPi.GPIO as gpio
except ImportError:
    exit("This service requires the RPi.GPIO module\nInstall with: sudo pip install RPi.GPIO")

try:
    import smbus
except ImportError:
    exit("This service requires the smbus module\nInstall with: sudo apt-get install python-smbus")


IGNORE_FACTOR = 0.6
INTERP_FACTOR = 0.51

WINDOW_SIZE = 5

LOW_CUT = 10
TOUCH_THRESHOLD = 50

DELAY = 1.0 / 120 # FPS

ROWS = 8
COLS = 14

H = 480
W = 800

STEP_Y = float(H) / (ROWS - 1)
STEP_X = float(W) / (COLS - 1)

data = [[0] * WINDOW_SIZE] * (ROWS + COLS)

os.system("sudo modprobe uinput")

DAEMON = True

CAPABILITIES = {
    e.EV_ABS : (
        (e.ABS_X, AbsInfo(value=0, min=0, max=800, fuzz=0, flat=0, resolution=1)),
        (e.ABS_Y, AbsInfo(value=0, min=0, max=480, fuzz=0, flat=0, resolution=1)),
        (e.ABS_MT_SLOT, AbsInfo(value=0, min=0, max=1, fuzz=0, flat=0, resolution=0)),
        (e.ABS_MT_TRACKING_ID, AbsInfo(value=0, min=0, max=65535, fuzz=0, flat=0, resolution=0)),
        (e.ABS_MT_POSITION_X, AbsInfo(value=0, min=0, max=800, fuzz=0, flat=0, resolution=0)),
        (e.ABS_MT_POSITION_Y, AbsInfo(value=0, min=0, max=480, fuzz=0, flat=0, resolution=0)),
    ),
    e.EV_KEY : [
        e.BTN_TOUCH, 
    ]
}

PIDFILE = "/var/run/hyperpixel-touch.pid"
LOGFILE = "/var/log/hyperpixel-touch.log"

if DAEMON:
    try:
        pid = os.fork()
        if pid > 0:
            sys.exit(0)

    except OSError, e:
        print("Fork #1 failed: {} ({})".format(e.errno, e.strerror))
        sys.exit(1)

    os.chdir("/")
    os.setsid()
    os.umask(0)

    try:
        pid = os.fork()
        if pid > 0:
            fpid = open(PIDFILE, 'w')
            fpid.write(str(pid))
            fpid.close()
            sys.exit(0)
    except OSError, e:
        print("Fork #2 failed: {} ({})".format(e.errno, e.strerror))
        sys.exit(1)

    si = file("/dev/null", 'r')
    so = file(LOGFILE, 'a+')
    se = file("/dev/null", 'a+', 0)

    os.dup2(si.fileno(), sys.stdin.fileno())
    os.dup2(so.fileno(), sys.stdout.fileno())
    os.dup2(se.fileno(), sys.stderr.fileno())

def log(msg):
    sys.stdout.write(str(datetime.now()))
    sys.stdout.write(": ")
    sys.stdout.write(msg)
    sys.stdout.write("\n")
    sys.stdout.flush()

try:
    ui = UInput(CAPABILITIES, name="Touchscreen", bustype=e.BUS_USB)

except uinput.UInputError as e:
    sys.stdout.write(e.message)
    sys.stdout.write("Have you tried running as root? sudo {}".format(sys.argv[0]))
    sys.exit(0)

INT = 27
ADDR = 0x5c

gpio.setmode(gpio.BCM)
gpio.setwarnings(False)
gpio.setup(INT, gpio.IN)

bus = smbus.SMBus(3)

last_status_one = last_status_two = False
last_status_x1 = last_status_y1 = last_status_x2 = last_status_y2 = 0

touch_one_start = None
touch_two_start = None
touch_one_end = 0
touch_two_end = 0
last_x1 = last_y1 = -1
last_x2 = last_y2 = -1

touch_on = False

def write_status(x1, y1, touch_one, x2, y2, touch_two):
    global last_status_one, last_status_two, last_status_x1, last_status_y1, last_status_x2, last_status_y2

    #print("{}:{} {}, {}:{} {}".format(x1, y1, touch_one, x2, y2, touch_two))
    if touch_one:
        ui.write(e.EV_ABS, e.ABS_MT_SLOT, 0)

        if not last_status_one: # Contact one press
            ui.write(e.EV_ABS, e.ABS_MT_TRACKING_ID, 0)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_X, x1)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_Y, y1)
            ui.write(e.EV_KEY, e.BTN_TOUCH, 1)
            ui.write(e.EV_ABS, e.ABS_X, x1)
            ui.write(e.EV_ABS, e.ABS_Y, y1)

        elif not last_status_one or (x1, y1) != (last_status_x1, last_status_y1):
            if x1 != last_status_x1: ui.write(e.EV_ABS, e.ABS_X, x1)
            if y1 != last_status_y1: ui.write(e.EV_ABS, e.ABS_Y, y1)
            ui.write(e.EV_KEY, e.BTN_TOUCH, 0)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_X, x1)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_Y, y1)

        last_status_x1 = x1
        last_status_y1 = y1

        last_status_one = True

        ui.write(e.EV_SYN, e.SYN_REPORT, 0)
        ui.syn()

    elif not touch_one and last_status_one: # Contact one release
        ui.write(e.EV_ABS, e.ABS_MT_SLOT, 0)
        ui.write(e.EV_KEY, e.BTN_TOUCH, 0)
        ui.write(e.EV_ABS, e.ABS_MT_TRACKING_ID, -1)
        last_status_one = False

        ui.write(e.EV_SYN, e.SYN_REPORT, 0)
        ui.syn()


    if touch_two:
        ui.write(e.EV_ABS, e.ABS_MT_SLOT, 1)

        if not last_status_two: # Contact one press
            ui.write(e.EV_ABS, e.ABS_MT_TRACKING_ID, 1)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_X, x2)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_Y, y2)
            ui.write(e.EV_KEY, e.BTN_TOUCH, 1)
            ui.write(e.EV_ABS, e.ABS_X, x2)
            ui.write(e.EV_ABS, e.ABS_Y, y2)

        elif not last_status_two or (x2, y2) != (last_status_x2, last_status_y2):
            if x2 != last_status_x2: ui.write(e.EV_ABS, e.ABS_X, x2)
            if y2 != last_status_y2: ui.write(e.EV_ABS, e.ABS_Y, y2)
            ui.write(e.EV_KEY, e.BTN_TOUCH, 0)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_X, x2)
            ui.write(e.EV_ABS, e.ABS_MT_POSITION_Y, y2)

        last_status_x2 = x2
        last_status_y2 = y2

        last_status_two = True

        ui.write(e.EV_SYN, e.SYN_REPORT, 0)
        ui.syn()

    elif not touch_two and last_status_two: # Contact one release
        ui.write(e.EV_ABS, e.ABS_MT_SLOT, 1)
        ui.write(e.EV_KEY, e.BTN_TOUCH, 0)
        ui.write(e.EV_ABS, e.ABS_MT_TRACKING_ID, -1)
        last_status_two = False

        ui.write(e.EV_SYN, e.SYN_REPORT, 0)
        ui.syn()

def custom_read_touch():
    raw_adc = bus.read_i2c_block_data(ADDR, 0x00, 32) + bus.read_i2c_block_data(ADDR, 0x20, 12)
    adc_out = [0] * (ROWS + COLS)

    if sum(raw_adc) < 100:
        return

    y = 0
    for x in range(0,len(raw_adc),2):
        val = (raw_adc[x] << 8) | raw_adc[x+1]
        val -= LOW_CUT
        val = max(0,int(val))

        #adc_out.append(val)
        data[y].insert(0, val)
        data[y] = data[y][:WINDOW_SIZE]
        y += 1

    for x in range(ROWS+COLS):
        val = 0
        for y in range(WINDOW_SIZE):
            val += data[x][y]
        adc_out[x] = int(val / WINDOW_SIZE)

    touch_x = list(reversed(adc_out[8:]))
    touch_y = adc_out[:8]
    touch_y[0] *= 0.90
    #touch_y[0] -= 50
    #touch_y[0] = max(touch_y[0], 0)

    #print(str(int(time.time() * 1000)) + "i: " + str(gpio.input(INT)) + " y: " + " ".join([str(x).rjust(4,' ') for x in touch_y]) + " | x: " + " ".join([str(x).rjust(4,' ') for x in touch_x]))
    touches = []
    found = []

    while len(touches) < 4:
        max_x = max(touch_x)
        max_y = max(touch_y)

        io_x = touch_x.index(max_x)
        io_y = touch_y.index(max_y)

        if max_x + max_y < TOUCH_THRESHOLD or (io_x, io_y) in found:
            break

        io_x = touch_x.index(max_x)
        io_y = touch_y.index(max_y)

        found.append((io_x, io_y))

        b_x = (STEP_X * io_x)
        b_y = (STEP_Y * io_y)

        if io_x < (COLS-1):
            b_x += (float(touch_x[io_x + 1]) / touch_x[io_x]) * (STEP_X * INTERP_FACTOR)
            touch_x[io_x + 1] *= IGNORE_FACTOR

        if io_x > 0:
            b_x -= (float(touch_x[io_x - 1]) / touch_x[io_x]) * (STEP_X * INTERP_FACTOR)
            touch_x[io_x - 1] *= IGNORE_FACTOR

        if io_y < (ROWS-1):
            b_y += (float(touch_y[io_y + 1]) / touch_y[io_y]) * (STEP_Y * INTERP_FACTOR)
            touch_y[io_y + 1] *= IGNORE_FACTOR

        if io_y > 0:
            b_y -= (float(touch_y[io_y - 1]) / touch_y[io_y]) * (STEP_Y * INTERP_FACTOR)
            touch_y[io_y - 1] *= IGNORE_FACTOR

        #print(touch_x, touch_y)
        touch_x[io_x] *= IGNORE_FACTOR
        touch_y[io_y] *= IGNORE_FACTOR

        touches.append((int(b_x), int(b_y)))
        #print(int(b_x), int(b_y))

    touches = sorted(touches, key=lambda touch: touch[0])
    x1, y1, x2, y2, t1, t2 = 0, 0, 0, 0, False, False

    if len(touches) > 0:
        x1, y1 = touches[0]
        t1 = True

    if len(touches) > 1:
        x2, y2 = touches[1]
        t2 = True

    write_status(x1, 480-y1, t1, x2, 480-y2, t2)

    global touch_on
    touch_on = t1 or t2

bus.write_byte_data(0x5c,0x6e,0b00001110)

log("HyperPixel Touch daemon running...")


try:
    while True:
        if gpio.input(INT) or touch_on: 
            custom_read_touch()
        elif touch_on:
            write_status(0, 480, False, 0, 480, False)
            touch_on = False

        time.sleep(DELAY)

except KeyboardInterrupt:
    pass

log("HyperPixel Touch daemon shutting down...")

ui.close()