From adf34c3414642611bb2c62ed2ff65a006f9ad8ca Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 25 Mar 2022 17:33:11 -0700 Subject: [PATCH] CarParams: remove max steer (#24044) * Remove max steer * Update ref --- cereal | 2 +- selfdrive/car/interfaces.py | 2 -- selfdrive/controls/lib/drive_helpers.py | 4 ---- selfdrive/controls/lib/latcontrol.py | 3 +++ selfdrive/controls/lib/latcontrol_indi.py | 7 ++----- selfdrive/controls/lib/latcontrol_lqr.py | 10 ++++------ selfdrive/controls/lib/latcontrol_pid.py | 8 +++----- selfdrive/test/process_replay/ref_commit | 2 +- 8 files changed, 14 insertions(+), 24 deletions(-) diff --git a/cereal b/cereal index e94649e0e..0b635b480 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e94649e0ed1f70fe5c74c3700a9330b878606101 +Subproject commit 0b635b4804c963c2e69475788acfeeb17820f8c2 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 452ea0095..199914a9b 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -75,8 +75,6 @@ class CarInterfaceBase(ABC): # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque - ret.steerMaxBP = [0.] - ret.steerMaxV = [1.] ret.minSteerSpeed = 0. ret.wheelSpeedFactor = 1.0 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 483889a46..7e01582db 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -40,10 +40,6 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def get_steer_max(CP, v_ego): - return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) - - def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index eb16aca2e..4a67715fc 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -12,6 +12,9 @@ class LatControl(ABC): self.sat_limit = CP.steerLimitTimer self.sat_count = 0. + # we define the steer torque scale as [-1.0...1.0] + self.steer_max = 1.0 + @abstractmethod def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): pass diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index dc1b31bad..2db2d4388 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,7 +5,6 @@ from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.drive_helpers import get_steer_max from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED @@ -41,7 +40,6 @@ class LatControlINDI(LatControl): self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL) - self.reset() @property @@ -107,8 +105,7 @@ class LatControlINDI(LatControl): output_steer = self.steer_filter.x + delta_u - steers_max = get_steer_max(CP, CS.vEgo) - output_steer = clip(output_steer, -steers_max, steers_max) + output_steer = clip(output_steer, -self.steer_max, self.steer_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) @@ -117,6 +114,6 @@ class LatControlINDI(LatControl): indi_log.delayedOutput = float(self.steer_filter.x) indi_log.delta = float(delta_u) indi_log.output = float(output_steer) - indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) + indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) return float(output_steer), float(steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 5c273a45b..583244882 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -4,7 +4,6 @@ import numpy as np from common.numpy_fast import clip from common.realtime import DT_CTRL from cereal import log -from selfdrive.controls.lib.drive_helpers import get_steer_max from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED @@ -34,7 +33,6 @@ class LatControlLQR(LatControl): def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): lqr_log = log.ControlsState.LateralLQRState.new_message() - steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed # Subtract offset. Zero angle should correspond to zero torque @@ -71,16 +69,16 @@ class LatControlLQR(LatControl): i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i - if (error >= 0 and (control <= steers_max or i < 0.0)) or \ - (error <= 0 and (control >= -steers_max or i > 0.0)): + if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \ + (error <= 0 and (control >= -self.steer_max or i > 0.0)): self.i_lqr = i output_steer = lqr_output + self.i_lqr - output_steer = clip(output_steer, -steers_max, steers_max) + output_steer = clip(output_steer, -self.steer_max, self.steer_max) lqr_log.steeringAngleDeg = angle_steers_k lqr_log.i = self.i_lqr lqr_log.output = output_steer lqr_log.lqrOutput = lqr_output - lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) + lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index f5ff5a95e..c711fa264 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,7 +1,6 @@ import math from selfdrive.controls.lib.pid import PIController -from selfdrive.controls.lib.drive_helpers import get_steer_max from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from cereal import log @@ -33,9 +32,8 @@ class LatControlPID(LatControl): pid_log.active = False self.pid.reset() else: - steers_max = get_steer_max(CP, CS.vEgo) - self.pid.pos_limit = steers_max - self.pid.neg_limit = -steers_max + self.pid.pos_limit = self.steer_max + self.pid.neg_limit = -self.steer_max # offset does not contribute to resistive torque steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) @@ -49,6 +47,6 @@ class LatControlPID(LatControl): pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer - pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 567a04353..65368e263 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5bf6cd4a8d3c96eb333ae58d81af3907ac8b2804 \ No newline at end of file +81ec7708ef7582ab950f9af4810d403fce292eb1 \ No newline at end of file