| 
						
						
						
					 | 
					 | 
					@ -1,7 +1,7 @@ | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#!/usr/bin/env python3 | 
					 | 
					 | 
					 | 
					#!/usr/bin/env python3 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					import os | 
					 | 
					 | 
					 | 
					import os | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					import math | 
					 | 
					 | 
					 | 
					import math | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from numbers import Number | 
					 | 
					 | 
					 | 
					from typing import SupportsFloat | 
				
			
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from cereal import car, log | 
					 | 
					 | 
					 | 
					from cereal import car, log | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from common.numpy_fast import clip | 
					 | 
					 | 
					 | 
					from common.numpy_fast import clip | 
				
			
			
		
	
	
		
		
			
				
					| 
						
						
						
							
								
							
						
					 | 
					 | 
					@ -17,6 +17,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					 | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.latcontrol import LatControl | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.longcontrol import LongControl | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.longcontrol import LongControl | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.latcontrol_pid import LatControlPID | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.latcontrol_pid import LatControlPID | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.latcontrol_indi import LatControlINDI | 
					 | 
					 | 
					 | 
					from selfdrive.controls.lib.latcontrol_indi import LatControlINDI | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
					 | 
					@ -137,6 +138,7 @@ class Controls: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    self.LoC = LongControl(self.CP) | 
					 | 
					 | 
					 | 
					    self.LoC = LongControl(self.CP) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    self.VM = VehicleModel(self.CP) | 
					 | 
					 | 
					 | 
					    self.VM = VehicleModel(self.CP) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					 | 
					 | 
					 | 
					 | 
					    self.LaC: LatControl | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    if self.CP.steerControlType == car.CarParams.SteerControlType.angle: | 
					 | 
					 | 
					 | 
					    if self.CP.steerControlType == car.CarParams.SteerControlType.angle: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      self.LaC = LatControlAngle(self.CP, self.CI) | 
					 | 
					 | 
					 | 
					      self.LaC = LatControlAngle(self.CP, self.CI) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    elif self.CP.lateralTuning.which() == 'pid': | 
					 | 
					 | 
					 | 
					    elif self.CP.lateralTuning.which() == 'pid': | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
					 | 
					@ -607,7 +609,7 @@ class Controls: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    # Ensure no NaNs/Infs | 
					 | 
					 | 
					 | 
					    # Ensure no NaNs/Infs | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    for p in ACTUATOR_FIELDS: | 
					 | 
					 | 
					 | 
					    for p in ACTUATOR_FIELDS: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      attr = getattr(actuators, p) | 
					 | 
					 | 
					 | 
					      attr = getattr(actuators, p) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      if not isinstance(attr, Number): | 
					 | 
					 | 
					 | 
					      if not isinstance(attr, SupportsFloat): | 
				
			
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        continue | 
					 | 
					 | 
					 | 
					        continue | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      if not math.isfinite(attr): | 
					 | 
					 | 
					 | 
					      if not math.isfinite(attr): | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
						
					 | 
					 | 
					
  |