mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-30 11:02:19 +08:00
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
This commit is contained in:
Shane Smiskol
@@ -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()
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user