From 6e6f7cbfc2ccf51fb493482db2aba34eec9d2e5a Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 13:26:24 +0100 Subject: [PATCH] INDI: replace internal rate limit by actuatorsOutput --- selfdrive/controls/lib/latcontrol_indi.py | 29 +++++++---------------- 1 file changed, 8 insertions(+), 21 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index c1f5384cc774ca..dc1b31bad9def9 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,8 +5,6 @@ from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.car import apply_toyota_steer_torque_limits -from selfdrive.car.toyota.values import CarControllerParams from selfdrive.controls.lib.drive_helpers import get_steer_max from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED @@ -37,8 +35,6 @@ def __init__(self, CP, CI): self.A_K = A - np.dot(K, C) self.x = np.array([[0.], [0.], [0.]]) - self.enforce_rate_limit = CP.carName == "toyota" - self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) @@ -67,7 +63,6 @@ def inner_loop_gain(self): def reset(self): super().reset() self.steer_filter.x = 0. - self.output_steer = 0. self.speed = 0. def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): @@ -90,12 +85,12 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, if CS.vEgo < MIN_STEER_SPEED or not active: indi_log.active = False - self.output_steer = 0.0 self.steer_filter.x = 0.0 + output_steer = 0 else: # Expected actuator value self.steer_filter.update_alpha(self.RC) - self.steer_filter.update(self.output_steer) + self.steer_filter.update(last_actuators.steer) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des @@ -107,21 +102,13 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, delta_u = g_inv * accel_error # If steering pressed, only allow wind down - if CS.steeringPressed and (delta_u * self.output_steer > 0): + if CS.steeringPressed and (delta_u * last_actuators.steer > 0): delta_u = 0 - # Enforce rate limit - if self.enforce_rate_limit: - steer_max = float(CarControllerParams.STEER_MAX) - new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u) - prev_output_steer_cmd = steer_max * self.output_steer - new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams) - self.output_steer = new_output_steer_cmd / steer_max - else: - self.output_steer = self.steer_filter.x + delta_u + output_steer = self.steer_filter.x + delta_u steers_max = get_steer_max(CP, CS.vEgo) - self.output_steer = clip(self.output_steer, -steers_max, steers_max) + output_steer = clip(output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) @@ -129,7 +116,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.steer_filter.x) indi_log.delta = float(delta_u) - indi_log.output = float(self.output_steer) - indi_log.saturated = self._check_saturation(steers_max - abs(self.output_steer) < 1e-3, CS) + indi_log.output = float(output_steer) + indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) - return float(self.output_steer), float(steers_des), indi_log + return float(output_steer), float(steers_des), indi_log