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
This commit is contained in:
parent
39fc171f91
commit
3a494e144b
|
@ -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,
|
||||
|
|
Loading…
Reference in a new issue