Move vCruise to card (#33439)
* Move vCruise to card * cleanup drive_helpers * works in sim * update refs * update that * too slow :( old-commit-hash: 2f3256ed8baa3d7a5c4d1befe9148dbfecb37246vw-mqb-aeb
Adeeb Shihadeh
1 year ago
committed by
GitHub
parent
1779ceb30e
commit
53288d4dbf
15 changed files with 198 additions and 182 deletions
@ -0,0 +1,132 @@ |
||||
import math |
||||
|
||||
from cereal import car |
||||
from openpilot.common.conversions import Conversions as CV |
||||
from openpilot.common.numpy_fast import clip |
||||
|
||||
|
||||
# WARNING: this value was determined based on the model's training distribution, |
||||
# model predictions above this speed can be unpredictable |
||||
# V_CRUISE's are in kph |
||||
V_CRUISE_MIN = 8 |
||||
V_CRUISE_MAX = 145 |
||||
V_CRUISE_UNSET = 255 |
||||
V_CRUISE_INITIAL = 40 |
||||
V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 |
||||
IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1) # round here to avoid rounding errors incrementing set speed |
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent |
||||
ButtonType = car.CarState.ButtonEvent.Type |
||||
CRUISE_LONG_PRESS = 50 |
||||
CRUISE_NEAREST_FUNC = { |
||||
ButtonType.accelCruise: math.ceil, |
||||
ButtonType.decelCruise: math.floor, |
||||
} |
||||
CRUISE_INTERVAL_SIGN = { |
||||
ButtonType.accelCruise: +1, |
||||
ButtonType.decelCruise: -1, |
||||
} |
||||
|
||||
|
||||
class VCruiseHelper: |
||||
def __init__(self, CP): |
||||
self.CP = CP |
||||
self.v_cruise_kph = V_CRUISE_UNSET |
||||
self.v_cruise_cluster_kph = V_CRUISE_UNSET |
||||
self.v_cruise_kph_last = 0 |
||||
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} |
||||
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} |
||||
|
||||
@property |
||||
def v_cruise_initialized(self): |
||||
return self.v_cruise_kph != V_CRUISE_UNSET |
||||
|
||||
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, enabled) |
||||
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_UNSET |
||||
self.v_cruise_cluster_kph = V_CRUISE_UNSET |
||||
|
||||
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 |
||||
|
||||
v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT |
||||
|
||||
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, timer in self.button_timers.items(): |
||||
if timer and timer % CRUISE_LONG_PRESS == 0: |
||||
button_type = k |
||||
long_press = True |
||||
break |
||||
|
||||
if button_type is None: |
||||
return |
||||
|
||||
# Don't adjust speed when pressing resume to exit standstill |
||||
cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill |
||||
if button_type == ButtonType.accelCruise and cruise_standstill: |
||||
return |
||||
|
||||
# Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge) |
||||
if not self.button_change_states[button_type]["enabled"]: |
||||
return |
||||
|
||||
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, enabled): |
||||
# 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: |
||||
# Start/end timer and store current state on change of button pressed |
||||
self.button_timers[b.type.raw] = 1 if b.pressed else 0 |
||||
self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} |
||||
|
||||
def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: |
||||
# initializing is handled by the PCM |
||||
if self.CP.pcmCruise: |
||||
return |
||||
|
||||
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL |
||||
|
||||
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized: |
||||
self.v_cruise_kph = self.v_cruise_kph_last |
||||
else: |
||||
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) |
||||
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph |
||||
@ -1 +1 @@ |
||||
ab4983de4477d648e5c36447debfd6735dfc466f |
||||
073dcca6932c5c66cdadf9aee9b531b623795888 |
||||
Loading…
Reference in new issue