From a7828e1ae3cf9259c850c26bbacf4f7ffe18253a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 11 Nov 2022 18:53:48 -0800 Subject: [PATCH] controlsd: add cruise speed helper class (#26472) * fix runaway set speed for GM * fix runaway set speed for GM * Handle resuming to exit standstill generically * clean that up * ugh i want to fix all the formatting * class that manages v_cruise * better name * move around * add depressed_state * fine to update on pressed change, better name * revert gm stuff * revert standstill stuff * remove * revert that * we can put this in here now! * below update * actually only used here * one line old-commit-hash: 811c096e6454dc2bbdd1e15a08952979c3fb7be1 --- selfdrive/controls/controlsd.py | 52 ++------- selfdrive/controls/lib/drive_helpers.py | 141 +++++++++++++++--------- 2 files changed, 102 insertions(+), 91 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3b241045b1..f69e9e7fd1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -16,8 +16,7 @@ from system.version import is_tested_branch, get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET -from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise -from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature +from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -49,7 +48,6 @@ Desire = log.LateralPlan.Desire LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection EventName = car.CarEvent.EventName -ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel @@ -173,9 +171,6 @@ class Controls: self.active = False self.can_rcv_timeout = False self.soft_disable_timer = 0 - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL - self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 self.can_rcv_timeout_counter = 0 @@ -185,11 +180,11 @@ class Controls: self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = None - self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} self.last_actuators = car.CarControl.Actuators.new_message() self.steer_limited = False self.desired_curvature = 0.0 self.desired_curvature_rate = 0.0 + self.v_cruise_helper = VCruiseHelper(self.CP) # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True @@ -219,7 +214,7 @@ class Controls: controls_state = Params().get("ReplayControlsState") if controls_state is not None: controls_state = log.ControlsState.from_bytes(controls_state) - self.v_cruise_kph = controls_state.vCruise + self.v_cruise_helper.v_cruise_kph = controls_state.vCruise if any(ps.controlsAllowed for ps in self.sm['pandaStates']): self.state = State.enabled @@ -245,7 +240,7 @@ class Controls: # Block resume if cruise never previously enabled resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and self.v_cruise_kph == V_CRUISE_INITIAL and resume_pressed: + if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: self.events.add(EventName.resumeBlocked) # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 @@ -478,20 +473,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_kph_last = self.v_cruise_kph - - if CS.cruiseState.available: - # if stock cruise is completely disabled, then we can use our own set speed logic - if not self.CP.pcmCruise: - self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, - self.button_timers, self.enabled, self.is_metric) - self.v_cruise_cluster_kph = self.v_cruise_kph - else: - self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH - else: - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -569,9 +551,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - if not self.CP.pcmCruise: - self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) - self.v_cruise_cluster_kph = self.v_cruise_kph + self.v_cruise_helper.initialize_v_cruise(CS) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -619,7 +599,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) @@ -683,16 +663,6 @@ class Controls: return CC, lac_log - def update_button_timers(self, buttonEvents): - # increment timer for buttons still pressed - for k in self.button_timers: - if self.button_timers[k] > 0: - self.button_timers[k] += 1 - - for b in buttonEvents: - if b.type.raw in self.button_timers: - self.button_timers[b.type.raw] = 1 if b.pressed else 0 - def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" @@ -715,7 +685,7 @@ class Controls: CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl - hudControl.setSpeed = float(self.v_cruise_cluster_kph * CV.KPH_TO_MS) + hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead @@ -798,8 +768,8 @@ class Controls: controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) - controlsState.vCruise = float(self.v_cruise_kph) - controlsState.vCruiseCluster = float(self.v_cruise_cluster_kph) + controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph) + controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) @@ -880,7 +850,6 @@ class Controls: self.publish_logs(CS, start_time, CC, lac_log) self.prof.checkpoint("Sent") - self.update_button_timers(CS.buttonEvents) self.CS_prev = CS def controlsd_thread(self): @@ -889,6 +858,7 @@ class Controls: self.rk.monitor_time() self.prof.display() + def main(sm=None, pm=None, logcan=None): controls = Controls(sm, pm, logcan) controls.controlsd_thread() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index e74be7199e..27e53d1dd3 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -22,6 +22,7 @@ CAR_ROTATION_RADIUS = 0.0 # EU guidelines MAX_LATERAL_JERK = 5.0 +ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type CRUISE_LONG_PRESS = 50 CRUISE_NEAREST_FUNC = { @@ -34,6 +35,96 @@ CRUISE_INTERVAL_SIGN = { } +class VCruiseHelper: + def __init__(self, CP): + self.CP = CP + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph_last = 0 + self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} + + @property + def v_cruise_initialized(self): + return self.v_cruise_kph != V_CRUISE_INITIAL + + def update_v_cruise(self, CS, enabled, is_metric): + self.v_cruise_kph_last = self.v_cruise_kph + + if CS.cruiseState.available: + if not self.CP.pcmCruise: + # if stock cruise is completely disabled, then we can use our own set speed logic + self._update_v_cruise_non_pcm(CS, enabled, is_metric) + self.v_cruise_cluster_kph = self.v_cruise_kph + self.update_button_timers(CS) + else: + self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + else: + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL + + def _update_v_cruise_non_pcm(self, CS, enabled, is_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 + if not enabled: + return + + long_press = False + button_type = None + + # should be CV.MPH_TO_KPH, but this causes rounding errors + v_cruise_delta = 1. if is_metric else 1.6 + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers and not b.pressed: + if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS: + return # end long press + button_type = b.type.raw + break + else: + for k in self.button_timers.keys(): + if self.button_timers[k] and self.button_timers[k] % CRUISE_LONG_PRESS == 0: + button_type = k + long_press = True + break + + if button_type: + v_cruise_delta = v_cruise_delta * (5 if long_press else 1) + if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval + self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta + else: + self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + + # If set is pressed while overriding, clip cruise speed to minimum of vEgo + if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): + self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) + + self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + + def update_button_timers(self, CS): + # increment timer for buttons still pressed + for k in self.button_timers: + if self.button_timers[k] > 0: + self.button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers: + self.button_timers[b.type.raw] = 1 if b.pressed else 0 + + def initialize_v_cruise(self, CS): + # initializing is handled by the PCM + if self.CP.pcmCruise: + return + + # 250kph or above probably means we never had a set speed + if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: + self.v_cruise_kph = self.v_cruise_kph_last + else: + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) + + self.v_cruise_cluster_kph = self.v_cruise_kph + + def apply_deadzone(error, deadzone): if error > deadzone: error -= deadzone @@ -48,56 +139,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 update_v_cruise(v_cruise_kph, v_ego, gas_pressed, 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 - if not enabled: - return v_cruise_kph - - long_press = False - button_type = None - - # should be CV.MPH_TO_KPH, but this causes rounding errors - v_cruise_delta = 1. if metric else 1.6 - - for b in buttonEvents: - if b.type.raw in button_timers and not b.pressed: - if button_timers[b.type.raw] > CRUISE_LONG_PRESS: - return v_cruise_kph # end long press - button_type = b.type.raw - break - else: - for k in button_timers.keys(): - if button_timers[k] and button_timers[k] % CRUISE_LONG_PRESS == 0: - button_type = k - long_press = True - break - - if button_type: - v_cruise_delta = v_cruise_delta * (5 if long_press else 1) - if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval - v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta - else: - v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] - - # If set is pressed while overriding, clip cruise speed to minimum of vEgo - if gas_pressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): - v_cruise_kph = max(v_cruise_kph, v_ego * CV.MS_TO_KPH) - - v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) - - return v_cruise_kph - - -def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): - for b in buttonEvents: - # 250kph or above probably means we never had a set speed - if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and v_cruise_last < 250: - return v_cruise_last - - return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) - - def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): if len(psis) != CONTROL_N: psis = [0.0]*CONTROL_N