diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d4352791ed..5f47cebb1f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -134,7 +134,8 @@ class LongitudinalPlanner: if self.mpc.mode == 'acc': accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] - accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ae8301871d..bcfc4d0c14 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -19,7 +19,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2', ignore_avg_freq=['radarState']) while True: @@ -30,7 +30,7 @@ def main(): ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) msg = messaging.new_message('driverAssistance') - msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2']) + msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'liveParameters']) msg.driverAssistance.leftLaneDeparture = ldw.left msg.driverAssistance.rightLaneDeparture = ldw.right pm.send('driverAssistance', msg) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 3c8c09d6be..c08ac6d369 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -64,6 +64,7 @@ class Plant: control = messaging.new_message('controlsState') ss = messaging.new_message('selfdriveState') car_state = messaging.new_message('carState') + lp = messaging.new_message('liveParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') a_lead = (v_lead - self.v_lead_prev)/self.ts @@ -130,6 +131,7 @@ class Plant: 'carControl': car_control.carControl, 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, + 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} self.planner.update(sm) self.speed = self.planner.v_desired_filter.x diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index ef17325642..448dc6896d 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -508,7 +508,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="plannerd", - pubs=["modelV2", "carControl", "carState", "controlsState", "radarState", "selfdriveState"], + pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"], subs=["longitudinalPlan", "driverAssistance"], ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1365eb1798..66abbd2803 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e7db12387eecd077c6eaca6622b7863ee1af7105 +22530fd1bd915d5b37db900e2ac42a9501cd5972