From 6518ecca8ff75475c6f79aac1af660c1ccc74da9 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 May 2022 16:59:52 -0700 Subject: [PATCH] Add desired curvature to controlsState (#24510) * Add desired_curvature to controlsState * clean up * bump * bump * update refs from GitHub! Co-authored-by: Jason Shuler --- selfdrive/controls/controlsd.py | 16 ++++++++++------ selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index cdf9f38e15..cf23ec6d80 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -167,6 +167,8 @@ class Controls: self.logged_comm_issue = None self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} self.last_actuators = car.CarControl.Actuators.new_message() + self.desired_curvature = 0.0 + self.desired_curvature_rate = 0.0 # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True @@ -571,13 +573,13 @@ class Controls: actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, - lat_plan.psis, - lat_plan.curvatures, - lat_plan.curvatureRates) + self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, + lat_plan.psis, + lat_plan.curvatures, + lat_plan.curvatureRates) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params, - self.last_actuators, desired_curvature, - desired_curvature_rate, self.sm['liveLocationKalman']) + self.last_actuators, self.desired_curvature, + self.desired_curvature_rate, self.sm['liveLocationKalman']) else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -721,6 +723,8 @@ class Controls: controlsState.enabled = self.enabled controlsState.active = self.active controlsState.curvature = curvature + controlsState.desiredCurvature = self.desired_curvature + controlsState.desiredCurvatureRate = self.desired_curvature_rate controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ac8f59b26b..5c039f66b8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -10b766fa845934f0258c52cdf2103d0e1a9496c9 \ No newline at end of file +16a3e2f9ef2bf013bc71bed6085a5296eb276f85