Add support for filtering in the qevdevtouch plugin

This patch adds a Kalman filter to the evdev touch plugin
which samples velocity and position in x and y direction
and uses this to emit touch points in sync with the application's
repaint rate. The filter also allows for basic prediction
based on current position and velocity estimates.

Filtering is opt-in, and can be enabled by passing for instance
 > app -plugin evdevtouch:/dev/touchscreen:filtered:prediction=16

The logic relies on a stable QWindow::requestUpdate() and will work
best in combination with QtQuick and blocking swap buffers (or
equivalent). QScreen::refreshRate() will also have to be reasonably
accurate.

[ChangeLog][Platform Specific Changes] The evdevtouch plugin now
has the option to apply filtering and prediction. Enabled by
passing "filtered" as an argument. Prediction can be specified
by passing "prediction=X" as an argument, where X is in
milliseconds.

Change-Id: I682db4386fe3a7cef8b4a08ea0d16c1491efb873
Reviewed-by: Laszlo Agocs <laszlo.agocs@qt.io>

1 files changed, 224 insertions, 37 deletions

diff --git a/src/platformsupport/input/evdevtouch/qevdevtouchhandler.cpp b/src/platformsupport/input/evdevtouch/qevdevtouchhandler.cpp
index 6870fd3dde..c515e663c8 100644
--- a/src/platformsupport/input/evdevtouch/qevdevtouchhandler.cpp
+++ b/src/platformsupport/input/evdevtouch/qevdevtouchhandler.cpp
@@ -1,6 +1,7 @@
 /****************************************************************************
 **
 ** Copyright (C) 2016 The Qt Company Ltd.
+** Copyright (C) 2016 Jolla Ltd, author: <gunnar.sletta@jollamobile.com>
 ** Contact: https://www.qt.io/licensing/
 **
 ** This file is part of the plugins module of the Qt Toolkit.
@@ -100,6 +101,7 @@ public:
     QEvdevTouchScreenHandler *q;
     int m_lastEventType;
     QList<QWindowSystemInterface::TouchPoint> m_touchPoints;
+    QList<QWindowSystemInterface::TouchPoint> m_lastTouchPoints;
 
     struct Contact {
         int trackingId;
@@ -118,11 +120,16 @@ public:
     Contact m_currentData;
     int m_currentSlot;
 
+    double m_timeStamp;
+    double m_lastTimeStamp;
+
     int findClosestContact(const QHash<int, Contact> &contacts, int x, int y, int *dist);
     void addTouchPoint(const Contact &contact, Qt::TouchPointStates *combinedStates);
     void reportPoints();
     void loadMultiScreenMappings();
 
+    QRect screenGeometry() const;
+
     int hw_range_x_min;
     int hw_range_x_max;
     int hw_range_y_min;
@@ -136,19 +143,40 @@ public:
     QTransform m_rotate;
     bool m_singleTouch;
     QString m_screenName;
-    QPointer<QScreen> m_screen;
+    mutable QPointer<QScreen> m_screen;
+
+    // Touch filtering and prediction are part of the same thing. The default
+    // prediction is 0ms, but sensible results can be acheived by setting it
+    // to, for instance, 16ms.
+    // For filtering to work well, the QPA plugin should provide a dead-steady
+    // implementation of QPlatformWindow::requestUpdate().
+    bool m_filtered;
+    int m_prediction;
+
+    // When filtering is enabled, protect the access to current and last
+    // timeStamp and touchPoints, as these are being read on the gui thread.
+    QMutex m_mutex;
 };
 
 QEvdevTouchScreenData::QEvdevTouchScreenData(QEvdevTouchScreenHandler *q_ptr, const QStringList &args)
     : q(q_ptr),
       m_lastEventType(-1),
       m_currentSlot(0),
+      m_timeStamp(0), m_lastTimeStamp(0),
       hw_range_x_min(0), hw_range_x_max(0),
       hw_range_y_min(0), hw_range_y_max(0),
       hw_pressure_min(0), hw_pressure_max(0),
-      m_typeB(false), m_singleTouch(false)
+      m_forceToActiveWindow(false), m_typeB(false), m_singleTouch(false),
+      m_filtered(false), m_prediction(0)
 {
-    m_forceToActiveWindow = args.contains(QLatin1String("force_window"));
+    for (const QString &arg : args) {
+        if (arg == QStringLiteral("force_window"))
+            m_forceToActiveWindow = true;
+        else if (arg == QStringLiteral("filtered"))
+            m_filtered = true;
+        else if (arg.startsWith(QStringLiteral("prediction=")))
+            m_prediction = arg.mid(11).toInt();
+    }
 }
 
 #define LONG_BITS (sizeof(long) << 3)
@@ -232,9 +260,14 @@ QEvdevTouchScreenHandler::QEvdevTouchScreenHandler(const QString &device, const
 #endif
 
     d->deviceNode = device;
-
-    qCDebug(qLcEvdevTouch, "evdevtouch: %s: Protocol type %c %s (%s)", qPrintable(d->deviceNode),
-            d->m_typeB ? 'B' : 'A', mtdevStr, d->m_singleTouch ? "single" : "multi");
+    qCDebug(qLcEvdevTouch,
+            "evdevtouch: %s: Protocol type %c %s (%s), filtered=%s",
+            qPrintable(d->deviceNode),
+            d->m_typeB ? 'B' : 'A', mtdevStr,
+            d->m_singleTouch ? "single" : "multi",
+            d->m_filtered ? "yes" : "no");
+    if (d->m_filtered)
+        qCDebug(qLcEvdevTouch, " - prediction=%d", d->m_prediction);
 
     input_absinfo absInfo;
     memset(&absInfo, 0, sizeof(input_absinfo));
@@ -331,6 +364,11 @@ QEvdevTouchScreenHandler::~QEvdevTouchScreenHandler()
     unregisterTouchDevice();
 }
 
+bool QEvdevTouchScreenHandler::isFiltered() const
+{
+    return d->m_filtered;
+}
+
 QTouchDevice *QEvdevTouchScreenHandler::touchDevice() const
 {
     return m_device;
@@ -520,6 +558,14 @@ void QEvdevTouchScreenData::processInputEvent(input_event *data)
         if (!m_contacts.isEmpty() && m_contacts.constBegin().value().trackingId == -1)
             assignIds();
 
+        if (m_filtered)
+            m_mutex.lock();
+
+        // update timestamps
+        m_lastTimeStamp = m_timeStamp;
+        m_timeStamp = data->time.tv_sec + data->time.tv_usec / 1000000.0;
+
+        m_lastTouchPoints = m_touchPoints;
         m_touchPoints.clear();
         Qt::TouchPointStates combinedStates;
 
@@ -597,8 +643,12 @@ void QEvdevTouchScreenData::processInputEvent(input_event *data)
         if (!m_typeB && !m_singleTouch)
             m_contacts.clear();
 
+
         if (!m_touchPoints.isEmpty() && combinedStates != Qt::TouchPointStationary)
             reportPoints();
+
+        if (m_filtered)
+            m_mutex.unlock();
     }
 
     m_lastEventType = data->type;
@@ -657,41 +707,45 @@ void QEvdevTouchScreenData::assignIds()
     m_contacts = newContacts;
 }
 
-void QEvdevTouchScreenData::reportPoints()
+QRect QEvdevTouchScreenData::screenGeometry() const
 {
-    QRect winRect;
     if (m_forceToActiveWindow) {
         QWindow *win = QGuiApplication::focusWindow();
-        if (!win)
-            return;
-        winRect = QHighDpi::toNativePixels(win->geometry(), win);
-    } else {
-        // Now it becomes tricky. Traditionally we picked the primaryScreen()
-        // and were done with it. But then, enter multiple screens, and
-        // suddenly it was all broken.
-        //
-        // For now we only support the display configuration of the KMS/DRM
-        // backends of eglfs. See QTouchOutputMapping.
-        //
-        // The good news it that once winRect refers to the correct screen
-        // geometry in the full virtual desktop space, there is nothing else
-        // left to do since qguiapp will handle the rest.
-        QScreen *screen = QGuiApplication::primaryScreen();
-        if (!m_screenName.isEmpty()) {
-            if (!m_screen) {
-                const QList<QScreen *> screens = QGuiApplication::screens();
-                for (QScreen *s : screens) {
-                    if (s->name() == m_screenName) {
-                        m_screen = s;
-                        break;
-                    }
+        return win ? QHighDpi::toNativePixels(win->geometry(), win) : QRect();
+    }
+
+    // Now it becomes tricky. Traditionally we picked the primaryScreen()
+    // and were done with it. But then, enter multiple screens, and
+    // suddenly it was all broken.
+    //
+    // For now we only support the display configuration of the KMS/DRM
+    // backends of eglfs. See QTouchOutputMapping.
+    //
+    // The good news it that once winRect refers to the correct screen
+    // geometry in the full virtual desktop space, there is nothing else
+    // left to do since qguiapp will handle the rest.
+    QScreen *screen = QGuiApplication::primaryScreen();
+    if (!m_screenName.isEmpty()) {
+        if (!m_screen) {
+            const QList<QScreen *> screens = QGuiApplication::screens();
+            for (QScreen *s : screens) {
+                if (s->name() == m_screenName) {
+                    m_screen = s;
+                    break;
                 }
             }
-            if (m_screen)
-                screen = m_screen;
         }
-        winRect = QHighDpi::toNativePixels(screen->geometry(), screen);
+        if (m_screen)
+            screen = m_screen;
     }
+    return QHighDpi::toNativePixels(screen->geometry(), screen);
+}
+
+void QEvdevTouchScreenData::reportPoints()
+{
+    QRect winRect = screenGeometry();
+    if (winRect.isNull())
+        return;
 
     const int hw_w = hw_range_x_max - hw_range_x_min;
     const int hw_h = hw_range_y_max - hw_range_y_min;
@@ -722,13 +776,17 @@ void QEvdevTouchScreenData::reportPoints()
     }
 
     // Let qguiapp pick the target window.
-    QWindowSystemInterface::handleTouchEvent(Q_NULLPTR, q->touchDevice(), m_touchPoints);
+    if (m_filtered)
+        emit q->touchPointsUpdated();
+    else
+        QWindowSystemInterface::handleTouchEvent(Q_NULLPTR, q->touchDevice(), m_touchPoints);
 }
 
-
-
 QEvdevTouchScreenHandlerThread::QEvdevTouchScreenHandlerThread(const QString &device, const QString &spec, QObject *parent)
     : QDaemonThread(parent), m_device(device), m_spec(spec), m_handler(Q_NULLPTR), m_touchDeviceRegistered(false)
+    , m_touchUpdatePending(false)
+    , m_filterWindow(Q_NULLPTR)
+    , m_touchRate(-1)
 {
     start();
 }
@@ -742,6 +800,10 @@ QEvdevTouchScreenHandlerThread::~QEvdevTouchScreenHandlerThread()
 void QEvdevTouchScreenHandlerThread::run()
 {
     m_handler = new QEvdevTouchScreenHandler(m_device, m_spec);
+
+    if (m_handler->isFiltered())
+        connect(m_handler, &QEvdevTouchScreenHandler::touchPointsUpdated, this, &QEvdevTouchScreenHandlerThread::scheduleTouchPointUpdate);
+
     // Report the registration to the parent thread by invoking the method asynchronously
     QMetaObject::invokeMethod(this, "notifyTouchDeviceRegistered", Qt::QueuedConnection);
 
@@ -762,5 +824,130 @@ void QEvdevTouchScreenHandlerThread::notifyTouchDeviceRegistered()
     emit touchDeviceRegistered();
 }
 
+void QEvdevTouchScreenHandlerThread::scheduleTouchPointUpdate()
+{
+    QWindow *window = QGuiApplication::focusWindow();
+    if (window != m_filterWindow) {
+        if (m_filterWindow)
+            m_filterWindow->removeEventFilter(this);
+        m_filterWindow = window;
+        if (m_filterWindow)
+            m_filterWindow->installEventFilter(this);
+    }
+    if (m_filterWindow) {
+        m_touchUpdatePending = true;
+        m_filterWindow->requestUpdate();
+    }
+}
+
+bool QEvdevTouchScreenHandlerThread::eventFilter(QObject *object, QEvent *event)
+{
+    if (m_touchUpdatePending && object == m_filterWindow && event->type() == QEvent::UpdateRequest) {
+        m_touchUpdatePending = false;
+        filterAndSendTouchPoints();
+    }
+    return false;
+}
+
+void QEvdevTouchScreenHandlerThread::filterAndSendTouchPoints()
+{
+    QRect winRect = m_handler->d->screenGeometry();
+    if (winRect.isNull())
+        return;
+
+    float vsyncDelta = 1.0f / QGuiApplication::primaryScreen()->refreshRate();
+
+    QHash<int, FilteredTouchPoint> filteredPoints;
+
+    m_handler->d->m_mutex.lock();
+
+    double time = m_handler->d->m_timeStamp;
+    double lastTime = m_handler->d->m_lastTimeStamp;
+    double touchDelta = time - lastTime;
+    if (m_touchRate < 0 || touchDelta > vsyncDelta) {
+        // We're at the very start, with nothing to go on, so make a guess
+        // that the touch rate will be somewhere in the range of half a vsync.
+        // This doesn't have to be accurate as we will calibrate it over time,
+        // but it gives us a better starting point so calibration will be
+        // slightly quicker. If, on the other hand, we already have an
+        // estimate, we'll leave it as is and keep it.
+        if (m_touchRate < 0)
+            m_touchRate = (1.0 / QGuiApplication::primaryScreen()->refreshRate()) / 2.0;
+
+    } else {
+        // Update our estimate for the touch rate. We're making the assumption
+        // that this value will be mostly accurate with the occational bump,
+        // so we're weighting the existing value high compared to the update.
+        const double ratio = 0.9;
+        m_touchRate = sqrt(m_touchRate * m_touchRate * ratio + touchDelta * touchDelta * (1.0 - ratio));
+    }
+
+    QList<QWindowSystemInterface::TouchPoint> points = m_handler->d->m_touchPoints;
+    const QList<QWindowSystemInterface::TouchPoint> &lastPoints = m_handler->d->m_lastTouchPoints;
+
+    m_handler->d->m_mutex.unlock();
+
+    for (int i=0; i<points.size(); ++i) {
+        QWindowSystemInterface::TouchPoint &tp = points[i];
+        QPointF pos = tp.normalPosition;
+        FilteredTouchPoint f;
+
+        QWindowSystemInterface::TouchPoint ltp;
+        ltp.id = -1;
+        for (int j=0; j<lastPoints.size(); ++j) {
+            if (lastPoints.at(j).id == tp.id) {
+                ltp = lastPoints.at(j);
+                break;
+            }
+        }
+
+        QPointF velocity;
+        if (lastTime != 0 && ltp.id >= 0)
+            velocity = (pos - ltp.normalPosition) / m_touchRate;
+        if (m_filteredPoints.contains(tp.id)) {
+            f = m_filteredPoints.take(tp.id);
+            f.x.update(pos.x(), velocity.x(), vsyncDelta);
+            f.y.update(pos.y(), velocity.y(), vsyncDelta);
+            pos = QPointF(f.x.position(), f.y.position());
+        } else {
+            f.x.initialize(pos.x(), velocity.x());
+            f.y.initialize(pos.y(), velocity.y());
+            // Make sure the first instance of a touch point we send has the
+            // 'pressed' state.
+            if (tp.state != Qt::TouchPointPressed)
+                tp.state = Qt::TouchPointPressed;
+        }
+
+        tp.velocity = QVector2D(f.x.velocity() * winRect.width(), f.y.velocity() * winRect.height());
+
+        tp.normalPosition = QPointF(f.x.position() + f.x.velocity() * m_handler->d->m_prediction / 1000.0,
+                                    f.y.position() + f.y.velocity() * m_handler->d->m_prediction / 1000.0);
+
+        qreal x = winRect.x() + (tp.normalPosition.x() * (winRect.width() - 1));
+        qreal y = winRect.y() + (tp.normalPosition.y() * (winRect.height() - 1));
+
+        tp.area.moveCenter(QPointF(x, y));
+
+        // Store the touch point for later so we can release it if we've
+        // missed the actual release between our last update and this.
+        f.touchPoint = tp;
+        filteredPoints[tp.id] = f;
+    }
+
+    for (QHash<int, FilteredTouchPoint>::const_iterator it = m_filteredPoints.constBegin(), end = m_filteredPoints.constEnd(); it != end; ++it) {
+        const FilteredTouchPoint &f = it.value();
+        QWindowSystemInterface::TouchPoint tp = f.touchPoint;
+        tp.state = Qt::TouchPointReleased;
+        tp.velocity = QVector2D();
+        points.append(tp);
+    }
+
+    m_filteredPoints = filteredPoints;
+
+    QWindowSystemInterface::handleTouchEvent(Q_NULLPTR,
+                                             m_handler->touchDevice(),
+                                             points);
+}
+
 
 QT_END_NAMESPACE