@ -21,7 +21,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot . selfdrive . car . car_helpers import get_startup_event
from openpilot . selfdrive . car . car_helpers import get_startup_event
from openpilot . selfdrive . car . card import CarD
from openpilot . selfdrive . car . card import CarD
from openpilot . selfdrive . controls . lib . alertmanager import AlertManager , set_offroad_alert
from openpilot . selfdrive . controls . lib . alertmanager import AlertManager , set_offroad_alert
from openpilot . selfdrive . controls . lib . drive_helpers import clip_curvature
from openpilot . selfdrive . controls . lib . drive_helpers import VCruiseHelper , clip_curvature
from openpilot . selfdrive . controls . lib . events import Events , ET
from openpilot . selfdrive . controls . lib . events import Events , ET
from openpilot . selfdrive . controls . lib . latcontrol import LatControl , MIN_LATERAL_CONTROL_SPEED
from openpilot . selfdrive . controls . lib . latcontrol import LatControl , MIN_LATERAL_CONTROL_SPEED
from openpilot . selfdrive . controls . lib . latcontrol_pid import LatControlPID
from openpilot . selfdrive . controls . lib . latcontrol_pid import LatControlPID
@ -143,6 +143,7 @@ class Controls:
self . desired_curvature = 0.0
self . desired_curvature = 0.0
self . experimental_mode = False
self . experimental_mode = False
self . personality = self . read_personality_param ( )
self . personality = self . read_personality_param ( )
self . v_cruise_helper = VCruiseHelper ( self . CP )
self . recalibrating_seen = False
self . recalibrating_seen = False
self . can_log_mono_time = 0
self . can_log_mono_time = 0
@ -168,7 +169,7 @@ class Controls:
controls_state = self . params . get ( " ReplayControlsState " )
controls_state = self . params . get ( " ReplayControlsState " )
if controls_state is not None :
if controls_state is not None :
with log . ControlsState . from_bytes ( controls_state ) as controls_state :
with log . ControlsState . from_bytes ( controls_state ) as controls_state :
self . card . v_cruise_helper . 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 ' ] ) :
if any ( ps . controlsAllowed for ps in self . sm [ ' pandaStates ' ] ) :
self . state = State . enabled
self . state = State . enabled
@ -197,6 +198,11 @@ class Controls:
if self . CP . passive :
if self . CP . passive :
return
return
# 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 not self . v_cruise_helper . v_cruise_initialized and resume_pressed :
self . events . add ( EventName . resumeBlocked )
if not self . CP . notCar :
if not self . CP . notCar :
self . events . add_from_msg ( self . sm [ ' driverMonitoringState ' ] . events )
self . events . add_from_msg ( self . sm [ ' driverMonitoringState ' ] . events )
@ -425,7 +431,7 @@ class Controls:
def state_transition ( self , CS ) :
def state_transition ( self , CS ) :
""" Compute conditional state transitions and execute actions on state transitions """
""" Compute conditional state transitions and execute actions on state transitions """
self . card . v_cruise_helper . update_v_cruise ( CS , self . enabled , self . is_metric )
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
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
# entrance in SOFT_DISABLING state
@ -500,7 +506,7 @@ class Controls:
else :
else :
self . state = State . enabled
self . state = State . enabled
self . current_alert_types . append ( ET . ENABLE )
self . current_alert_types . append ( ET . ENABLE )
self . card . v_cruise_helper . initialize_v_cruise ( CS , self . experimental_mode )
self . v_cruise_helper . initialize_v_cruise ( CS , self . experimental_mode )
# Check if openpilot is engaged and actuators are enabled
# Check if openpilot is engaged and actuators are enabled
self . enabled = self . state in ENABLED_STATES
self . enabled = self . state in ENABLED_STATES
@ -556,7 +562,7 @@ class Controls:
if not self . joystick_mode :
if not self . joystick_mode :
# accel PID loop
# accel PID loop
pid_accel_limits = self . CI . get_pid_accel_limits ( self . CP , CS . vEgo , self . card . v_cruise_helper . 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 . recv_frame [ ' longitudinalPlan ' ] ) * DT_CTRL
t_since_plan = ( self . sm . frame - self . sm . recv_frame [ ' longitudinalPlan ' ] ) * DT_CTRL
actuators . accel = self . LoC . update ( CC . longActive , CS , long_plan , pid_accel_limits , t_since_plan )
actuators . accel = self . LoC . update ( CC . longActive , CS , long_plan , pid_accel_limits , t_since_plan )
@ -659,7 +665,7 @@ class Controls:
CC . cruiseControl . resume = self . enabled and CS . cruiseState . standstill and speeds [ - 1 ] > 0.1
CC . cruiseControl . resume = self . enabled and CS . cruiseState . standstill and speeds [ - 1 ] > 0.1
hudControl = CC . hudControl
hudControl = CC . hudControl
hudControl . setSpeed = float ( self . card . v_cruise_helper . 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 . speedVisible = self . enabled
hudControl . lanesVisible = self . enabled
hudControl . lanesVisible = self . enabled
hudControl . leadVisible = self . sm [ ' longitudinalPlan ' ] . hasLead
hudControl . leadVisible = self . sm [ ' longitudinalPlan ' ] . hasLead
@ -743,8 +749,8 @@ class Controls:
controlsState . engageable = not self . events . contains ( ET . NO_ENTRY )
controlsState . engageable = not self . events . contains ( ET . NO_ENTRY )
controlsState . longControlState = self . LoC . long_control_state
controlsState . longControlState = self . LoC . long_control_state
controlsState . vPid = float ( self . LoC . v_pid )
controlsState . vPid = float ( self . LoC . v_pid )
controlsState . vCruise = float ( self . card . v_cruise_helper . v_cruise_kph )
controlsState . vCruise = float ( self . v_cruise_helper . v_cruise_kph )
controlsState . vCruiseCluster = float ( self . card . v_cruise_helper . v_cruise_cluster_kph )
controlsState . vCruiseCluster = float ( self . v_cruise_helper . v_cruise_cluster_kph )
controlsState . upAccelCmd = float ( self . LoC . pid . p )
controlsState . upAccelCmd = float ( self . LoC . pid . p )
controlsState . uiAccelCmd = float ( self . LoC . pid . i )
controlsState . uiAccelCmd = float ( self . LoC . pid . i )
controlsState . ufAccelCmd = float ( self . LoC . pid . f )
controlsState . ufAccelCmd = float ( self . LoC . pid . f )