sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

controlsd: add cruise speed helper class (#26472)

Browse Source 
* 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
pull/26476/head^2
Shane Smiskol 2 years ago committed by GitHub
parent 89b88fc7a2
commit 811c096e64
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 102 additions and 91 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 52
      selfdrive/controls/controlsd.py
  2. 141
      selfdrive/controls/lib/drive_helpers.py

52
selfdrive/controls/controlsd.py
Unescape View File

@ -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()

141
selfdrive/controls/lib/drive_helpers.py
Unescape View File

@ -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

Write Preview
Loading…
Cancel
Save