tici: take into account frame timestamp (#2199)

* compensate for frame processing time

* add rolling shutter time

* Add max frame delay of 250ms

* only delay on tici
old-commit-hash: 3a494e144b

selfdrive/controls/lib/pathplanner.py

@@ -7,6 +7,7 @@ from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
 from selfdrive.controls.lib.lane_planner import LanePlanner
 from selfdrive.config import Conversions as CV
 from common.params import Params
+from common.hardware import TICI
 import cereal.messaging as messaging
 from cereal import log
 
@@ -164,8 +165,14 @@ class PathPlanner():
       self.LP.r_prob *= self.lane_change_ll_prob
     self.LP.update_d_poly(v_ego)
 
-    # account for actuation delay
-    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
+    if TICI:
+      frame_delay = min((sm.logMonoTime['model'] - sm['model'].timestampEof) / 1e9, 0.250)
+      delay = frame_delay + CP.steerActuatorDelay
+    else:
+      delay = CP.steerActuatorDelay
+
+    # account for actuation + frame delay
+    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, delay)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,