diff --git a/cereal b/cereal index 95f9fa186f..49a0ee9196 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 95f9fa186fc48277acd4c2d7202765485a9fb1a8 +Subproject commit 49a0ee9196ce013d422febdf61847d87f1f57627 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 6bf105072b..d438e98eee 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -73,9 +73,12 @@ class CarInterfaceBase(): ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA ret.openpilotLongitudinalControl = False ret.minSpeedCan = 0.3 - ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop + ret.startAccel = -0.8 + ret.stopAccel = -2.0 ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart + ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop ret.vEgoStopping = 0.5 + ret.vEgoStarting = 0.5 ret.stoppingControl = True ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 45218d6150..8c8dbc99d6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -16,7 +16,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature -from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED +from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR @@ -318,7 +318,7 @@ class Controls: v_future = speeds[-1] else: v_future = 100.0 - if CS.brakePressed and v_future >= STARTING_TARGET_SPEED \ + if CS.brakePressed and v_future >= self.CP.vEgoStarting \ and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3: self.events.add(EventName.noTarget) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index c2874ed2c3..6f5571073d 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,5 +1,6 @@ from cereal import car from common.numpy_fast import clip, interp +from common.realtime import DT_CTRL from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.drive_helpers import CONTROL_N from selfdrive.modeld.constants import T_IDXS @@ -7,12 +8,6 @@ from selfdrive.modeld.constants import T_IDXS LongCtrlState = car.CarControl.Actuators.LongControlState STOPPING_TARGET_SPEED_OFFSET = 0.01 -STARTING_TARGET_SPEED = 0.5 -DECEL_THRESHOLD_TO_PID = 0.8 - -DECEL_STOPPING_TARGET = 2.0 # apply at least this amount of brake to maintain the vehicle stationary - -RATE = 100.0 # As per ISO 15622:2018 for all speeds ACCEL_MIN_ISO = -3.5 # m/s^2 @@ -28,7 +23,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_ ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or brake_pressed)) - starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill + starting_condition = v_target > CP.vEgoStarting and not cruise_standstill if not active: long_control_state = LongCtrlState.off @@ -49,7 +44,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_ elif long_control_state == LongCtrlState.starting: if stopping_condition: long_control_state = LongCtrlState.stopping - elif output_accel >= -DECEL_THRESHOLD_TO_PID: + elif output_accel >= CP.startAccel: long_control_state = LongCtrlState.pid return long_control_state @@ -60,7 +55,7 @@ class LongControl(): self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - rate=RATE, + rate=1/DT_CTRL, sat_limit=0.8) self.v_pid = 0.0 self.last_output_accel = 0.0 @@ -119,16 +114,16 @@ class LongControl(): # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped - if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET: - output_accel -= CP.stoppingDecelRate / RATE + if not CS.standstill or output_accel > CP.stopAccel: + output_accel -= CP.stoppingDecelRate * DT_CTRL output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) self.reset(CS.vEgo) # Intention is to move again, release brake fast before handing control to PID elif self.long_control_state == LongCtrlState.starting: - if output_accel < -DECEL_THRESHOLD_TO_PID: - output_accel += CP.startingAccelRate / RATE + if output_accel < CP.startAccel: + output_accel += CP.startingAccelRate * DT_CTRL self.reset(CS.vEgo) self.last_output_accel = output_accel diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 75d21ce0f7..25d7e3e0f9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -65c39ad66072b5106399605761c48e6cad0ee3a5 \ No newline at end of file +0ab64a741a496f0bc7351d0e95364e8a192a87e9 \ No newline at end of file