|
|
|
@ -7,7 +7,6 @@ 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 |
|
|
|
|
|
|
|
|
@ -165,14 +164,8 @@ class PathPlanner(): |
|
|
|
|
self.LP.r_prob *= self.lane_change_ll_prob |
|
|
|
|
self.LP.update_d_poly(v_ego) |
|
|
|
|
|
|
|
|
|
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) |
|
|
|
|
# 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) |
|
|
|
|
|
|
|
|
|
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, |
|
|
|
|