diff --git a/cereal b/cereal index ff49c8e126..a197e38c0c 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 e737b4d2e7..83eed611c3 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -39,7 +39,7 @@ class CarInterface(CarInterfaceBase): 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 e97ed6b056..3de6daae21 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 91981259aa..1fecdd7c63 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 47f0b69f2b..12714082a6 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,6 +4,7 @@ from cereal import log 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 @@ LOW_SPEED_FACTOR = 200 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,10 +42,7 @@ class LatControlTorque(LatControl): self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction self.kf = CP.lateralTuning.torque.kf - - def reset(self): - super().reset() - self.pid.reset() + 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() @@ -54,24 +53,27 @@ class LatControlTorque(LatControl): 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 + #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 # convert friction into lateral accel units for feedforward - friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + friction_compensation = interp(error, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) ff += friction_compensation / self.kf freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f2cd282198..e9458607d5 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_MIN_ISO = -3.5 # m/s^2 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""" diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 298de7999a..613b7f26ae 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -935abbc21282e10572c9324382feba3fee2fe379 \ No newline at end of file +123506cad1877e93bfe5c91ecdce654ef339959b