@ -1,16 +1,16 @@ 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  cereal  import  car  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  opendbc . can . packer  import  CANPacker  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  selfdrive . car  import  apply_toyota_steer_torque_limits  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  selfdrive . car . chrysler . chryslercan  import  create_lkas_hud ,  create_lkas_command ,  \ 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					                                               create_wheel_buttons   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  selfdrive . car . chrysler . chryslercan  import  create_lkas_hud ,  create_lkas_command ,  create_wheel_buttons  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  selfdrive . car . chrysler . values  import  CAR ,  CarControllerParams  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					from  opendbc . can . packer  import  CANPacker  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					class  CarController ( ) :  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					class  CarController :  
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					  def  __init__ ( self ,  dbc_name ,  CP ,  VM ) :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . CP  =  CP   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . apply_steer_last  =  0   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . cc frame  =  0   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . prev_frame  =  - 1   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . frame  =  0   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . prev_lkas_ frame  =  - 1   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . hud_count  =  0   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . car_fingerprint  =  CP . carFingerprint   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . gone_fast_yet  =  False   
				
			 
			
		
	
	
		
			
				
					
						
						
						
							
								 
							 
						
					 
				
				 
				 
				
					@ -18,12 +18,13 @@ class CarController(): 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . packer  =  CANPacker ( dbc_name )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					  def  update ( self ,  enabled ,  CS ,  actuators ,  pcm_cancel_cmd ,  hud_alert ) :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					  def  update ( self ,  CC ,  CS ) :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    # this seems needed to avoid steering faults and to force the sync with the EPS counter   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    frame  =  CS . lkas_counter   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  self . prev_frame  ==  frame :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  self . prev_lkas_frame  ==  CS . lkas_counter :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      return  car . CarControl . Actuators . new_message ( ) ,  [ ]   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    actuators  =  CC . actuators   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    # steer torque   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    new_steer  =  int ( round ( actuators . steer  *  CarControllerParams . STEER_MAX ) )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    apply_steer  =  apply_toyota_steer_torque_limits ( new_steer ,  self . apply_steer_last ,   
				
			 
			
		
	
	
		
			
				
					
						
						
						
							
								 
							 
						
					 
				
				 
				 
				
					@ -36,7 +37,7 @@ class CarController(): 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    elif  self . car_fingerprint  in  ( CAR . PACIFICA_2019_HYBRID ,  CAR . PACIFICA_2020 ,  CAR . JEEP_CHEROKEE_2019 ) :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      if  CS . out . vEgo  <  ( self . CP . minSteerSpeed  -  3.0 ) :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					        self . gone_fast_yet  =  False   # < 14.5m/s stock turns off this bit, but fine down to 13.5   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    lkas_active  =  moving_fast  and  enabled   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    lkas_active  =  moving_fast  and  CC . enabled   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  not  lkas_active :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      apply_steer  =  0   
				
			 
			
		
	
	
		
			
				
					
						
						
						
							
								 
							 
						
					 
				
				 
				 
				
					@ -45,28 +46,24 @@ class CarController(): 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    can_sends  =  [ ]   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    #*** control msgs ***   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    #  *** control msgs ***   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  pcm_cancel_cmd :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  CC . cruiseControl . cancel :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      # TODO: would be better to start from frame_2b3   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      new_msg  =  create_wheel_buttons ( self . packer ,  self . ccframe ,  cancel = True )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      can_sends . append ( new_msg )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      can_sends . append ( create_wheel_buttons ( self . packer ,  self . frame ,  cancel = True ) )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    # LKAS_HEARTBIT is forwarded by Panda so no need to send it here.   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    # frame is 100Hz (0.01s period)   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  ( self . ccframe  %  25  ==  0 ) :   # 0.25s period   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      if  ( CS . lkas_car_model  !=  - 1 ) :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					        new_msg  =  create_lkas_hud (   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					            self . packer ,  CS . out . gearShifter ,  lkas_active ,  hud_alert ,   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					            self . hud_count ,  CS . lkas_car_model )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					        can_sends . append ( new_msg )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    if  self . frame  %  25  ==  0 :   # 0.25s period   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					      if  CS . lkas_car_model  !=  - 1 :   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					        can_sends . append ( create_lkas_hud ( self . packer ,  CS . out . gearShifter ,  lkas_active ,   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					                                         CC . hudControl . visualAlert ,  self . hud_count ,  CS . lkas_car_model ) )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					        self . hud_count  + =  1   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    new_msg  =  create_lkas_command ( self . packer ,  int ( apply_steer ) ,  self . gone_fast_yet ,  frame )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    can_sends . append ( new_msg )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    can_sends . append ( create_lkas_command ( self . packer ,  int ( apply_steer ) ,  self . gone_fast_yet ,  CS . lkas_counter ) )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . cc frame  + =  1   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . prev_frame  =  frame   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . frame  + =  1   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    self . prev_lkas_ frame  =  CS . lkas_counter   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					
 
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    new_actuators  =  actuators . copy ( )   
				
			 
			
		
	
		
			
				
					 
					 
				
				 
				 
				
					    new_actuators . steer  =  apply_steer  /  CarControllerParams . STEER_MAX