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