From 74927ed09b69f512d9a8c53fae4dec79c45c85ca Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Fri, 10 Jun 2022 13:48:15 -0700 Subject: [PATCH] Add deadzone --- cereal | 2 +- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/toyota/tunes.py | 4 ++-- selfdrive/controls/lib/drive_helpers.py | 16 +++++++++++++--- selfdrive/controls/lib/latcontrol_torque.py | 10 ++++++++-- selfdrive/controls/lib/longcontrol.py | 12 +----------- 6 files changed, 26 insertions(+), 20 deletions(-) diff --git a/cereal b/cereal index ff49c8e126ea04..a197e38c0cef00 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit ff49c8e126ea04889af13d690ceaf520ce7084d2 +Subproject commit a197e38c0cef00ececf4c7500438d37659d9199e diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index e737b4d2e7a441..83eed611c31f42 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -39,7 +39,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.maxLateralAccel = 1.7 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06, steering_angle_deadzone_deg=1.0) elif candidate == CAR.PRIUS_V: stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index e97ed6b056792a..3de6daae21b468 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -50,9 +50,9 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, use_steering_angle=True): +def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True): if name == LatTunes.TORQUE: - set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) + set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION, steering_angle_deadzone_deg) elif 'PID' in str(name): tune.init('pid') tune.pid.kiBP = [0.0] diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 91981259aa04a2..1fecdd7c638a1a 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -38,6 +38,16 @@ class MPC_COST_LAT: STEER_RATE = 1.0 +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + + def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) @@ -98,10 +108,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N v_ego = max(v_ego, 0.1) - + # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - + # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature @@ -109,7 +119,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): psi = interp(delay, T_IDXS[:CONTROL_N], psis) average_curvature_desired = psi / (v_ego * delay) desired_curvature = 2 * average_curvature_desired - current_curvature_desired - + # This is the "desired rate of the setpoint" not an actual desired rate desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 1078af30b9e79c..12714082a61eb9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,6 +4,7 @@ from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from selfdrive.controls.lib.pid import PIDController +from selfdrive.controls.lib.drive_helpers import apply_deadzone from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # At higher speeds (25+mph) we can assume: @@ -22,13 +23,14 @@ JERK_THRESHOLD = 0.2 -def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01): +def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0): tune.init('torque') tune.torque.useSteeringAngle = True tune.torque.kp = 1.0 / MAX_LAT_ACCEL tune.torque.kf = 1.0 / MAX_LAT_ACCEL tune.torque.ki = 0.1 / MAX_LAT_ACCEL tune.torque.friction = FRICTION + tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg class LatControlTorque(LatControl): @@ -40,6 +42,7 @@ def __init__(self, CP, CI): self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction self.kf = CP.lateralTuning.torque.kf + self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() @@ -50,19 +53,22 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi else: if self.use_steering_angle: actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) else: actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) + curvature_deadzone = 0.0 desired_lateral_accel = desired_curvature * CS.vEgo ** 2 # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature #desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature - error = setpoint - measurement + error = apply_deadzone(setpoint - measurement, lateral_accel_deadzone) pid_log.error = error ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f2cd28219812fe..e9458607d5d1c0 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,7 +1,7 @@ from cereal import car from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.drive_helpers import CONTROL_N +from selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone from selfdrive.controls.lib.pid import PIDController from selfdrive.modeld.constants import T_IDXS @@ -12,16 +12,6 @@ ACCEL_MAX_ISO = 2.0 # m/s^2 -def apply_deadzone(error, deadzone): - if error > deadzone: - error -= deadzone - elif error < - deadzone: - error += deadzone - else: - error = 0. - return error - - def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_target_future, brake_pressed, cruise_standstill): """Update longitudinal control state machine"""