1#!/usr/bin/env python 2# 3# Copyright 2016 The Chromium OS Authors. All rights reserved. 4# 5# Licensed under the Apache License, Version 2.0 (the "License"); 6# you may not use this file except in compliance with the License. 7# You may obtain a copy of the License at 8# 9# http://www.apache.org/licenses/LICENSE-2.0 10# 11# Unless required by applicable law or agreed to in writing, software 12# distributed under the License is distributed on an "AS IS" BASIS, 13# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14# See the License for the specific language governing permissions and 15# limitations under the License. 16# 17 18""" 19Module for computing drag latency given logs of touchpad positions and 20QuickStep laser crossing timestamps 21""" 22 23import numpy 24import evparser 25 26debug_mode = False 27 28 29def load_laser_data(fname_laser): 30 laser_data = numpy.loadtxt(fname_laser) 31 t = laser_data[:, 0] 32 transition = laser_data[:, 1].astype(int) 33 if transition[0] != 0: 34 print('WARNING: First laser transition should be from light to dark') 35 return t, transition 36 37 38def calc_ssr(x, y): 39 """Return sum of squared residuals (SSR) of a linear least square fit""" 40 p = numpy.polyfit(x, y, 1, full=True) 41 r = p[1][0] 42 return r 43 44 45def minimize_lsq(tx, x, ty, y, tl, min_shift, max_shift, step): 46 """Find best time shift so that the shifted laser crossing events fit nicely 47 on a straight line. Upper and lower side are treated separately. 48 49 """ 50 51 # generate an array of all shifts to try 52 shifts = numpy.arange(min_shift, max_shift, step) 53 54 # side = [0, 1, 1, 0, 0, 1, 1 ... 55 # this is an indicator of which side of the beam the crossing belongs to 56 side = ((numpy.arange(len(tl)) + 1) / 2) % 2 57 58 residuals0 = [] 59 residuals1 = [] 60 for shift in shifts: 61 # Find the locations of the finger at the shifted laser timestamps 62 yl = numpy.interp(tl + shift, ty, y) 63 xl = numpy.interp(tl + shift, tx, x) 64 # Fit a line to each side separately and save the SSR for this fit 65 residuals0.append(calc_ssr(xl[side == 0], yl[side == 0])) 66 residuals1.append(calc_ssr(xl[side == 1], yl[side == 1])) 67 68 # Find the shift with lower SSR for each side 69 best_shift0 = shifts[numpy.argmin(residuals0)] 70 best_shift1 = shifts[numpy.argmin(residuals1)] 71 72 # Use average of the two sides 73 best_shift = (best_shift0 + best_shift1) / 2 74 return best_shift 75 76 77def minimize(fname_evtest, fname_laser): 78 79 # Load all the data 80 tl, transition = load_laser_data(fname_laser) 81 (tx, x, ty, y) = evparser.load_xy(fname_evtest) 82 83 # Shift time so that first time point is 0 84 t0 = min(tx[0], ty[0]) 85 tx = tx - t0 86 ty = ty - t0 87 tl = tl - t0 88 89 # Sanity checks 90 if numpy.std(x)*2 < numpy.std(y): 91 print('WARNING: Not enough motion in X axis') 92 93 # Search for minimum with coarse step of 1 ms in range of 0 to 200 ms 94 coarse_step = 1e-3 # Seconds 95 best_shift_coarse = minimize_lsq(tx, x, ty, y, tl, 0, 0.2, coarse_step) 96 # Run another search with 0.02 ms step within +-3 ms of the previous result 97 lmts = numpy.array([-1, 1]) * 3 * coarse_step + best_shift_coarse 98 fine_step = 2e-5 # seconds 99 best_shift_fine = minimize_lsq(tx, x, ty, y, tl, lmts[0], lmts[1], fine_step) 100 101 print("Drag latency (min method) = %.2f ms" % (best_shift_fine*1000)) 102 if debug_mode: 103 debug_plot(tx, x, ty, y, tl, best_shift_fine) 104 105 return best_shift_fine 106 107 108def debug_plot(tx, x, ty, y, tl, shift): 109 """Plot the XY data with time-shifted laser events 110 111 Note: this is a utility function used for offline debugging. It needs 112 matplotlib which is not installed on CrOS images. 113 114 """ 115 import matplotlib.pyplot as plt 116 xx = numpy.interp(ty, tx, x) 117 plt.plot(xx, y, '.b') 118 119 yl = numpy.interp(tl + shift, ty, y) 120 xl = numpy.interp(tl + shift, tx, x) 121 sides = (((numpy.arange(len(tl)) + 1) / 2) % 2) 122 colors = ['g', 'm'] 123 x_linear = numpy.array([min(x), max(x)]) 124 for side in [0, 1]: 125 xls = xl[sides == side] 126 yls = yl[sides == side] 127 plt.plot(xls, yls, 'o' + colors[side]) 128 a, c = numpy.polyfit(xls, yls, 1) 129 plt.plot(x_linear, a * x_linear + c, colors[side]) 130 plt.xlabel('X') 131 plt.ylabel('Y') 132 plt.title('Laser events shifted %.2f ms' % (shift*1000)) 133 plt.show() 134 135# Debug & test 136if __name__ == '__main__': 137 138 fname = '/tmp/WALT_2016_06_22__1739_21_' 139 fname_evtest = fname + 'evtest.log' 140 fname_laser = fname + 'laser.log' 141 142 minimize(fname_evtest, fname_laser) 143 144