@ -13,8 +13,8 @@ import cereal.messaging as messaging
from cereal . visionipc import VisionIpcClient , VisionStreamType from cereal . visionipc import VisionIpcClient , VisionStreamType
from openpilot . common . conversions import Conversions as CV from openpilot . common . conversions import Conversions as CV
from panda import ALTERNATIVE_EXPERIENCE from panda import ALTERNATIVE_EXPERIENCE
from openpilot . system . swaglog import cloudlog from openpilot . common . swaglog import cloudlog
from openpilot . system . version import is_release_branch , get_short_branch from openpilot . system . version import get_short_branch
from openpilot . selfdrive . boardd . boardd import can_list_to_can_capnp from openpilot . selfdrive . boardd . boardd import can_list_to_can_capnp
from openpilot . selfdrive . car . car_helpers import get_car , get_startup_event , get_one_can from openpilot . selfdrive . car . car_helpers import get_car , get_startup_event , get_one_can
from openpilot . selfdrive . controls . lib . lateral_planner import CAMERA_OFFSET from openpilot . selfdrive . controls . lib . lateral_planner import CAMERA_OFFSET
@ -64,15 +64,13 @@ class Controls:
# Setup sockets # Setup sockets
self . pm = messaging . PubMaster ( [ ' sendcan ' , ' controlsState ' , ' carState ' , self . pm = messaging . PubMaster ( [ ' sendcan ' , ' controlsState ' , ' carState ' ,
' carControl ' , ' car Events' , ' carParams ' ] ) ' carControl ' , ' onroad Events' , ' carParams ' ] )
self . sensor_packets = [ " accelerometer " , " gyroscope " ] self . sensor_packets = [ " accelerometer " , " gyroscope " ]
self . camera_packets = [ " roadCameraState " , " driverCameraState " , " wideRoadCameraState " ] self . camera_packets = [ " roadCameraState " , " driverCameraState " , " wideRoadCameraState " ]
can_timeout = None if os . environ . get ( ' NO_CAN_TIMEOUT ' , False ) else 20
self . can_sock = messaging . sub_sock ( ' can ' , timeout = can_timeout )
self . log_sock = messaging . sub_sock ( ' androidLog ' ) self . log_sock = messaging . sub_sock ( ' androidLog ' )
self . can_sock = messaging . sub_sock ( ' can ' , timeout = 20 )
self . params = Params ( ) self . params = Params ( )
ignore = self . sensor_packets + [ ' testJoystick ' ] ignore = self . sensor_packets + [ ' testJoystick ' ]
@ -82,7 +80,7 @@ class Controls:
' driverMonitoringState ' , ' longitudinalPlan ' , ' lateralPlan ' , ' liveLocationKalman ' , ' driverMonitoringState ' , ' longitudinalPlan ' , ' lateralPlan ' , ' liveLocationKalman ' ,
' managerState ' , ' liveParameters ' , ' radarState ' , ' liveTorqueParameters ' , ' managerState ' , ' liveParameters ' , ' radarState ' , ' liveTorqueParameters ' ,
' testJoystick ' ] + self . camera_packets + self . sensor_packets , ' testJoystick ' ] + self . camera_packets + self . sensor_packets ,
ignore_alive = ignore , ignore_avg_freq = [ ' radarState ' , ' testJoystick ' ] ) ignore_alive = ignore , ignore_avg_freq = [ ' radarState ' , ' testJoystick ' ] , ignore_valid = [ ' testJoystick ' , ] )
if CI is None : if CI is None :
# wait for one pandaState and one CAN packet # wait for one pandaState and one CAN packet
@ -90,12 +88,13 @@ class Controls:
get_one_can ( self . can_sock ) get_one_can ( self . can_sock )
num_pandas = len ( messaging . recv_one_retry ( self . sm . sock [ ' pandaStates ' ] ) . pandaStates ) num_pandas = len ( messaging . recv_one_retry ( self . sm . sock [ ' pandaStates ' ] ) . pandaStates )
experimental_long_allowed = self . params . get_bool ( " ExperimentalLongitudinalEnabled " ) and not is_release_branch ( ) experimental_long_allowed = self . params . get_bool ( " ExperimentalLongitudinalEnabled " )
self . CI , self . CP = get_car ( self . can_sock , self . pm . sock [ ' sendcan ' ] , experimental_long_allowed , num_pandas ) self . CI , self . CP = get_car ( self . can_sock , self . pm . sock [ ' sendcan ' ] , experimental_long_allowed , num_pandas )
else : else :
self . CI , self . CP = CI , CI . CP self . CI , self . CP = CI , CI . CP
self . joystick_mode = self . params . get_bool ( " JoystickDebugMode " ) or self . CP . notCar self . joystick_enabled = self . params . get_bool ( " JoystickDebugMode " )
self . joystick_mode = self . joystick_enabled or self . CP . notCar
# set alternative experiences from parameters # set alternative experiences from parameters
self . disengage_on_accelerator = self . params . get_bool ( " DisengageOnAccelerator " ) self . disengage_on_accelerator = self . params . get_bool ( " DisengageOnAccelerator " )
@ -115,8 +114,8 @@ class Controls:
car_recognized = self . CP . carName != ' mock ' car_recognized = self . CP . carName != ' mock '
controller_available = self . CI . CC is not None and not passive and not self . CP . dashcamOnly controller_available = self . CI . CC is not None and not passive and not self . CP . dashcamOnly
self . read_only = not car_recognized or not controller_available or self . CP . dashcamOnly self . CP . passive = not car_recognized or not controller_available or self . CP . dashcamOnly
if self . read_only : if self . CP . passive :
safety_config = car . CarParams . SafetyConfig . new_message ( ) safety_config = car . CarParams . SafetyConfig . new_message ( )
safety_config . safetyModel = car . CarParams . SafetyModel . noOutput safety_config . safetyModel = car . CarParams . SafetyModel . noOutput
self . CP . safetyConfigs = [ safety_config ] self . CP . safetyConfigs = [ safety_config ]
@ -133,7 +132,7 @@ class Controls:
put_nonblocking ( " CarParamsPersistent " , cp_bytes ) put_nonblocking ( " CarParamsPersistent " , cp_bytes )
# cleanup old params # cleanup old params
if not self . CP . experimentalLongitudinalAvailable or is_release_branch ( ) : if not self . CP . experimentalLongitudinalAvailable :
self . params . remove ( " ExperimentalLongitudinalEnabled " ) self . params . remove ( " ExperimentalLongitudinalEnabled " )
if not self . CP . openpilotLongitudinalControl : if not self . CP . openpilotLongitudinalControl :
self . params . remove ( " ExperimentalMode " ) self . params . remove ( " ExperimentalMode " )
@ -179,8 +178,6 @@ class Controls:
self . v_cruise_helper = VCruiseHelper ( self . CP ) self . v_cruise_helper = VCruiseHelper ( self . CP )
self . recalibrating_seen = False self . recalibrating_seen = False
# TODO: no longer necessary, aside from process replay
self . sm [ ' liveParameters ' ] . valid = True
self . can_log_mono_time = 0 self . can_log_mono_time = 0
self . startup_event = get_startup_event ( car_recognized , controller_available , len ( self . CP . carFw ) > 0 ) self . startup_event = get_startup_event ( car_recognized , controller_available , len ( self . CP . carFw ) > 0 )
@ -193,7 +190,7 @@ class Controls:
set_offroad_alert ( " Offroad_CarUnrecognized " , True ) set_offroad_alert ( " Offroad_CarUnrecognized " , True )
else : else :
set_offroad_alert ( " Offroad_NoFirmware " , True ) set_offroad_alert ( " Offroad_NoFirmware " , True )
elif self . read_only : elif self . CP . passive :
self . events . add ( EventName . dashcamMode , static = True ) self . events . add ( EventName . dashcamMode , static = True )
elif self . joystick_mode : elif self . joystick_mode :
self . events . add ( EventName . joystickDebug , static = True ) self . events . add ( EventName . joystickDebug , static = True )
@ -214,7 +211,7 @@ class Controls:
self . state = State . enabled self . state = State . enabled
def update_events ( self , CS ) : def update_events ( self , CS ) :
""" Compute car Events from carState """ """ Compute onroad Events from carState """
self . events . clear ( ) self . events . clear ( )
@ -229,7 +226,7 @@ class Controls:
return return
# no more events while in dashcam mode # no more events while in dashcam mode
if self . read_only : if self . CP . passive :
return return
# Block resume if cruise never previously enabled # Block resume if cruise never previously enabled
@ -374,16 +371,17 @@ class Controls:
else : else :
self . logged_comm_issue = None self . logged_comm_issue = None
if not self . sm [ ' lateralPlan ' ] . mpcSolutionValid : if not ( self . CP . notCar and self . joystick_enabled ) :
self . events . add ( EventName . plannerError ) if not self . sm [ ' lateralPlan ' ] . mpcSolutionValid :
if not self . sm [ ' liveLocationKalman ' ] . posenetOK : self . events . add ( EventName . plannerError )
self . events . add ( EventName . posenetInvalid ) if not self . sm [ ' liveLocationKalman ' ] . posenetOK :
if not self . sm [ ' liveLocationKalman ' ] . deviceStable : self . events . add ( EventName . posenetInvalid )
self . events . add ( EventName . deviceFalling ) if not self . sm [ ' liveLocationKalman ' ] . deviceStable :
if not self . sm [ ' liveLocationKalman ' ] . inputsOK : self . events . add ( EventName . deviceFalling )
self . events . add ( EventName . locationdTemporaryError ) if not self . sm [ ' liveLocationKalman ' ] . inputsOK :
if not self . sm [ ' liveParameters ' ] . valid and not TESTING_CLOSET and ( not SIMULATION or REPLAY ) : self . events . add ( EventName . locationdTemporaryError )
self . events . add ( EventName . paramsdTemporaryError ) if not self . sm [ ' liveParameters ' ] . valid and not TESTING_CLOSET and ( not SIMULATION or REPLAY ) :
self . events . add ( EventName . paramsdTemporaryError )
# conservative HW alert. if the data or frequency are off, locationd will throw an error # conservative HW alert. if the data or frequency are off, locationd will throw an error
if any ( ( self . sm . frame - self . sm . rcv_frame [ s ] ) * DT_CTRL > 10. for s in self . sensor_packets ) : if any ( ( self . sm . frame - self . sm . rcv_frame [ s ] ) * DT_CTRL > 10. for s in self . sensor_packets ) :
@ -444,7 +442,7 @@ class Controls:
if VisionStreamType . VISION_STREAM_WIDE_ROAD not in available_streams : if VisionStreamType . VISION_STREAM_WIDE_ROAD not in available_streams :
self . sm . ignore_alive . append ( ' wideRoadCameraState ' ) self . sm . ignore_alive . append ( ' wideRoadCameraState ' )
if not self . read_only : if not self . CP . passive :
self . CI . init ( self . CP , self . can_sock , self . pm . sock [ ' sendcan ' ] ) self . CI . init ( self . CP , self . can_sock , self . pm . sock [ ' sendcan ' ] )
self . initialized = True self . initialized = True
@ -618,7 +616,7 @@ class Controls:
lat_plan . curvatures , lat_plan . curvatures ,
lat_plan . curvatureRates ) lat_plan . curvatureRates )
actuators . steer , actuators . steeringAngleDeg , lac_log = self . LaC . update ( CC . latActive , CS , self . VM , lp , actuators . steer , actuators . steeringAngleDeg , lac_log = self . LaC . update ( CC . latActive , CS , self . VM , lp ,
self . last_actuators , self . steer_limited , self . desired_curvature , self . steer_limited , self . desired_curvature ,
self . desired_curvature_rate , self . sm [ ' liveLocationKalman ' ] ) self . desired_curvature_rate , self . sm [ ' liveLocationKalman ' ] )
actuators . curvature = self . desired_curvature actuators . curvature = self . desired_curvature
else : else :
@ -637,7 +635,7 @@ class Controls:
if CC . latActive : if CC . latActive :
steer = clip ( joystick_axes [ 1 ] , - 1 , 1 ) steer = clip ( joystick_axes [ 1 ] , - 1 , 1 )
# max angle is 45 for angle-based cars, max curvature is 0.02 # max angle is 45 for angle-based cars, max curvature is 0.02
actuators . steer , actuators . steeringAngleDeg , actuators . curvature = steer , steer * 45 ., steer * - 0.02 actuators . steer , actuators . steeringAngleDeg , actuators . curvature = steer , steer * 90 ., steer * - 0.02
lac_log . active = self . active lac_log . active = self . active
lac_log . steeringAngleDeg = CS . steeringAngleDeg lac_log . steeringAngleDeg = CS . steeringAngleDeg
@ -749,7 +747,7 @@ class Controls:
if current_alert : if current_alert :
hudControl . visualAlert = current_alert . visual_alert hudControl . visualAlert = current_alert . visual_alert
if not self . read_only and self . initialized : if not self . CP . passive and self . initialized :
# send car controls over can # send car controls over can
now_nanos = self . can_log_mono_time if REPLAY else int ( time . monotonic ( ) * 1e9 ) now_nanos = self . can_log_mono_time if REPLAY else int ( time . monotonic ( ) * 1e9 )
self . last_actuators , can_sends = self . CI . apply ( CC , now_nanos ) self . last_actuators , can_sends = self . CI . apply ( CC , now_nanos )
@ -825,16 +823,18 @@ class Controls:
cs_send . carState . events = car_events cs_send . carState . events = car_events
self . pm . send ( ' carState ' , cs_send ) self . pm . send ( ' carState ' , cs_send )
# car Events - logged every second or on change # onroad Events - logged every second or on change
if ( self . sm . frame % int ( 1. / DT_CTRL ) == 0 ) or ( self . events . names != self . events_prev ) : if ( self . sm . frame % int ( 1. / DT_CTRL ) == 0 ) or ( self . events . names != self . events_prev ) :
ce_send = messaging . new_message ( ' carEvents ' , len ( self . events ) ) ce_send = messaging . new_message ( ' onroadEvents ' , len ( self . events ) )
ce_send . carEvents = car_events ce_send . valid = True
self . pm . send ( ' carEvents ' , ce_send ) ce_send . onroadEvents = car_events
self . pm . send ( ' onroadEvents ' , ce_send )
self . events_prev = self . events . names . copy ( ) self . events_prev = self . events . names . copy ( )
# carParams - logged every 50 seconds (> 1 per segment) # carParams - logged every 50 seconds (> 1 per segment)
if ( self . sm . frame % int ( 50. / DT_CTRL ) == 0 ) : if ( self . sm . frame % int ( 50. / DT_CTRL ) == 0 ) :
cp_send = messaging . new_message ( ' carParams ' ) cp_send = messaging . new_message ( ' carParams ' )
cp_send . valid = True
cp_send . carParams = self . CP cp_send . carParams = self . CP
self . pm . send ( ' carParams ' , cp_send ) self . pm . send ( ' carParams ' , cp_send )
@ -862,7 +862,7 @@ class Controls:
self . update_events ( CS ) self . update_events ( CS )
cloudlog . timestamp ( " Events updated " ) cloudlog . timestamp ( " Events updated " )
if not self . read_only and self . initialized : if not self . CP . passive and self . initialized :
# Update control state # Update control state
self . state_transition ( CS ) self . state_transition ( CS )
self . prof . checkpoint ( " State transition " ) self . prof . checkpoint ( " State transition " )
Shane Smiskol
2 years ago