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 stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.enableCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
elif self.CP.enableCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_transition(frame, CS, CP, state, events, soft_disable_timer,
v_cruise_kph, AM, events_arne182, arne_sm):
"""Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
v_cruise_kph_last = v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not CP.enableCruise:
v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
elif CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
df_out = df_manager.update()
if df_out.changed:
df_alert = 'dfButtonAlert'
if df_out.is_auto and df_out.last_is_auto:
if CS.cruiseState.enabled and not hide_auto_df_alerts:
df_alert += 'NoSound'
AM.add(frame,
df_alert,
enabled,
extra_text_1=df_out.model_profile_text + ' (auto)',
extra_text_2='Dynamic follow: {} profile active'.format(
df_out.model_profile_text))
else:
AM.add(frame,
df_alert,
enabled,
extra_text_1=df_out.user_profile_text,
extra_text_2='Dynamic follow: {} profile active'.format(
df_out.user_profile_text))
if traffic_light_alerts:
traffic_status = arne_sm['trafficModelEvent'].status
traffic_confidence = round(
arne_sm['trafficModelEvent'].confidence * 100, 2)
if traffic_confidence >= 75:
if traffic_status == 'SLOW':
AM.add(frame,
'trafficSlow',
enabled,
extra_text_2=' ({}%)'.format(traffic_confidence))
elif traffic_status == 'GREEN':
AM.add(frame,
'trafficGreen',
enabled,
extra_text_2=' ({}%)'.format(traffic_confidence))
elif traffic_status == 'DEAD': # confidence will be 100
AM.add(frame, 'trafficDead', enabled)
# DISABLED
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(frame, str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add(frame, "enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents,
v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
# DISABLED
if state == State.disabled:
if get_events(events_arne182, [ET.ENABLE]):
if get_events(events_arne182, [ET.NO_ENTRY]):
for e in get_events(events_arne182, [ET.NO_ENTRY]):
AM.add(frame, str(e) + "NoEntry", enabled)
else:
if get_events(events_arne182, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add(frame, "enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo,
CS.buttonevents_arne182,
v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events_arne182, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif get_events(events_arne182, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events_arne182, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events_arne182, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events_arne182, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif get_events(events_arne182,
[ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events_arne182, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events_arne182, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events_arne182,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events_arne182,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events_arne182, [ET.PRE_ENABLE]):
state = State.enabled
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_transition(frame, CS, CP, state, events, soft_disable_timer,
v_cruise_kph, AM):
"""Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
v_cruise_kph_last = v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not CP.enableCruise:
v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
elif CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# DISABLED
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(frame, str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add(frame, "enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents,
v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
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 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)
else:
if CS.cruiseState.available:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
else:
self.v_cruise_kph = 0
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.events.any(ET.OVERRIDE):
self.state = State.overriding
self.current_alert_types.append(ET.OVERRIDE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if self.events.any(ET.NO_ENTRY):
self.state = State.disabled
self.current_alert_types.append(ET.NO_ENTRY)
elif not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# OVERRIDING
elif self.state == State.overriding:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif not self.events.any(ET.OVERRIDE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.OVERRIDE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
elif self.events.any(ET.OVERRIDE):
self.state = State.overriding
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)
# Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES
self.active = self.state in ACTIVE_STATES
if self.active:
self.current_alert_types.append(ET.WARNING)
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
# if stock cruise is completely disabled, then we can use our own set speed logic
# self.CP.enableCruise is true
self.CP.enableCruise = self.CI.CP.enableCruise
if not self.CP.enableCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
if int(CS.vSetDis)-1 > self.v_cruise_kph:
self.v_cruise_kph = int(CS.vSetDis)
elif self.CP.enableCruise and CS.cruiseState.enabled:
if Params().get_bool('OpkrVariableCruise') and CS.cruiseState.modeSel != 0 and self.CP.vCruisekph > 30:
self.v_cruise_kph = self.CP.vCruisekph
self.v_cruise_kph_last = self.v_cruise_kph
elif CS.cruiseButtons == Buttons.RES_ACCEL and Params().get_bool('OpkrVariableCruise') and CS.cruiseState.modeSel != 0 and CS.vSetDis < (self.v_cruise_kph_last - 1):
self.v_cruise_kph = self.v_cruise_kph_last
if int(CS.vSetDis)-1 > self.v_cruise_kph:
self.v_cruise_kph = int(CS.vSetDis)
elif CS.cruiseButtons == Buttons.RES_ACCEL and Params().get_bool('OpkrVariableCruise') and CS.cruiseState.modeSel != 0 and 30 <= self.v_cruise_kph_last <= round(CS.vEgo*CV.MS_TO_KPH):
self.v_cruise_kph = round(CS.vEgo*CV.MS_TO_KPH)
if int(CS.vSetDis)-1 > self.v_cruise_kph:
self.v_cruise_kph = int(CS.vSetDis)
self.v_cruise_kph_last = self.v_cruise_kph
elif CS.cruiseButtons == Buttons.RES_ACCEL or CS.cruiseButtons == Buttons.SET_DECEL:
self.v_cruise_kph = round(CS.cruiseState.speed * CV.MS_TO_KPH)
self.v_cruise_kph_last = self.v_cruise_kph
elif CS.driverAcc and Params().get_bool('OpkrVariableCruise') and Params().get_bool('CruiseOverMaxSpeed') and 30 <= self.v_cruise_kph < int(round(CS.vEgo*CV.MS_TO_KPH)):
self.v_cruise_kph = int(round(CS.vEgo*CV.MS_TO_KPH))
self.v_cruise_kph_last = self.v_cruise_kph
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
#self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
self.v_cruise_kph = 0
self.v_cruise_kph_last = 0
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph,
time_gap, AM):
"""Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
v_cruise_kph_last = v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not CP.enableCruise:
v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
elif CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# DISABLED
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add("enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents,
v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
# Openpilot Buttons -- https://github.com/rhinodavid/OpenpilotButtons
# if there has been an acc time gap button press, advance the time gap
for e in CS.buttonEvents:
if e.type == "accTimeGapButton" and e.pressed == True:
time_gap = TimeGaps.advance(time_gap)
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last, time_gap
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
cur_time = sec_since_boot()
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.pcmCruise:
for b in CS.buttonEvents:
if b.pressed:
if b.type == car.CarState.ButtonEvent.Type.accelCruise:
self.accel_pressed = True
self.accel_pressed_last = cur_time
elif b.type == car.CarState.ButtonEvent.Type.decelCruise:
self.decel_pressed = True
self.decel_pressed_last = cur_time
else:
if b.type == car.CarState.ButtonEvent.Type.accelCruise:
self.accel_pressed = False
elif b.type == car.CarState.ButtonEvent.Type.decelCruise:
self.decel_pressed = False
v_cruise = self.v_cruise_kph if self.is_metric else int(
round((float(self.v_cruise_kph) * 0.6233 + 0.0995)))
vEgo = getattr(CS, "vEgo", None)
vEgo = int(
round((float(vEgo) * 3.6 if self.is_metric else int(
round((float(vEgo) * 3.6 * 0.6233 +
0.0995)))))) if vEgo else v_cruise
self.CI.CS.speed_limit_active = (
self.sm['longitudinalPlan'].speedLimitControlState ==
log.LongitudinalPlan.SpeedLimitControlState.active)
if self.CI.CS.speed_limit_active:
self.CI.CS.speed_limit = (
self.sm['longitudinalPlan'].speedLimit +
self.sm['longitudinalPlan'].speedLimitOffset
) * 3.6 # convert to kph
if self.CI.CS.one_pedal_mode_engage_on_gas:
self.CI.CS.one_pedal_mode_engage_on_gas = False
self.CI.CS.one_pedal_v_cruise_kph_last = self.v_cruise_kph
self.CI.CS.one_pedal_last_follow_level = self.CI.CS.follow_level
self.v_cruise_kph = V_CRUISE_MIN
elif not self.accel_pressed and cur_time - self.accel_pressed_last < 0.3 and self.enabled and CS.cruiseState.enabled and (
self.CI.CS.one_pedal_mode_active
or self.CI.CS.coast_one_pedal_mode_active
) and self.CI.CS.one_pedal_v_cruise_kph_last > 0: # user resuming speed from one-pedal mode
self.v_cruise_kph = self.CI.CS.one_pedal_v_cruise_kph_last
self.CI.CS.one_pedal_brake_mode = min(
1, self.CI.CS.one_pedal_last_brake_mode)
self.CI.CS.follow_level = self.CI.CS.one_pedal_last_follow_level
else:
self.v_cruise_kph = update_v_cruise(
v_cruise, CS.buttonEvents, self.enabled
and CS.cruiseState.enabled, cur_time, self.accel_pressed,
self.decel_pressed, self.accel_pressed_last,
self.decel_pressed_last, self.fastMode,
self.fast_mode_enabled, vEgo, self.v_cruise_last_changed)
self.v_cruise_kph = self.v_cruise_kph if self.is_metric else int(
round((float(round(self.v_cruise_kph)) - 0.0995) / 0.6233))
if self.v_cruise_kph < self.v_cruise_kph_last and cur_time - self.v_cruise_last_changed > 1.0:
self.CI.CS.one_pedal_v_cruise_kph_last = self.v_cruise_kph + (
self.v_cruise_kph_last - self.v_cruise_kph)
if self.v_cruise_kph != self.v_cruise_kph_last:
self.v_cruise_last_changed = cur_time
if (self.accel_pressed or self.decel_pressed):
if self.v_cruise_kph_last != self.v_cruise_kph:
self.accel_pressed_last = cur_time
self.decel_pressed_last = cur_time
self.fastMode = True
else:
self.fastMode = False
elif self.CP.pcmCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
if self.events.any(ET.RESET_V_CRUISE):
self.v_cruise_kph = 0
self.CI.CS.v_cruise_kph = self.v_cruise_kph
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(
ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(
CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_transition(self, CS, events):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.enableCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
elif self.CP.enableCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
alert_types = []
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if get_events(events, [ET.USER_DISABLE]):
self.state = State.disabled
self.AM.add(self.sm.frame, "disable", self.enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
self.state = State.disabled
alert_types = [ET.IMMEDIATE_DISABLE]
else:
# ENABLED
if self.state == State.enabled:
if get_events(events, [ET.SOFT_DISABLE]):
self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
alert_types = [ET.SOFT_DISABLE]
# SOFT DISABLING
elif self.state == State.softDisabling:
if not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and self.soft_disable_timer > 0:
alert_types = [ET.SOFT_DISABLE]
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not get_events(events, [ET.PRE_ENABLE]):
self.state = State.enabled
# DISABLED
elif self.state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
self.AM.add(self.sm.frame, str(e) + "NoEntry", self.enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
self.state = State.preEnabled
else:
self.state = State.enabled
self.AM.add(self.sm.frame, "enable", self.enabled)
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
for e in get_events(events, alert_types):
self.AM.add(self.sm.frame, e, self.enabled)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
