Revert "tici: take into account frame timestamp (#2199)"

This reverts commit 58dc2982af6593d7a31464342d80190fa365287e.

old-commit-hash: 65e5032a97
commatwo_master
Willem Melching 5 years ago
parent 58f07d754c
commit 2b95a9fec2
  1. 11
      selfdrive/controls/lib/pathplanner.py

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

Loading…
Cancel
Save