@ -459,7 +459,7 @@ class Controls:
self . v_cruise_kph = initialize_v_cruise ( CS . vEgo , CS . buttonEvents , self . v_cruise_kph_last ) self . v_cruise_kph = initialize_v_cruise ( CS . vEgo , CS . buttonEvents , self . v_cruise_kph_last )
# Check if actuators are enabled # Check if actuators are enabled
self . active = self . state == State . enabled or self . state == State . softDisabling self . active = self . state in ( State . enabled , State . softDisabling )
if self . active : if self . active :
self . current_alert_types . append ( ET . WARNING ) self . current_alert_types . append ( ET . WARNING )
@ -467,7 +467,7 @@ class Controls:
self . enabled = self . active or self . state == State . preEnabled self . enabled = self . active or self . state == State . preEnabled
def state_control ( self , CS ) : def state_control ( self , CS ) :
""" Given the state, this function returns an actuators packet """ """ Given the state, this function returns a CarControl packet """
# Update VehicleModel # Update VehicleModel
params = self . sm [ ' liveParameters ' ] params = self . sm [ ' liveParameters ' ]
@ -478,7 +478,14 @@ class Controls:
lat_plan = self . sm [ ' lateralPlan ' ] lat_plan = self . sm [ ' lateralPlan ' ]
long_plan = self . sm [ ' longitudinalPlan ' ] long_plan = self . sm [ ' longitudinalPlan ' ]
actuators = car . CarControl . Actuators . new_message ( ) CC = car . CarControl . new_message ( )
CC . enabled = self . enabled
# Check which actuators can be enabled
CC . latActive = self . active and not CS . steerFaultTemporary and not CS . steerFaultPermanent and \
CS . vEgo > self . CP . minSteerSpeed and not CS . standstill
CC . longActive = self . active
actuators = CC . actuators
actuators . longControlState = self . LoC . long_control_state actuators . longControlState = self . LoC . long_control_state
if CS . leftBlinker or CS . rightBlinker : if CS . leftBlinker or CS . rightBlinker :
@ -486,37 +493,40 @@ class Controls:
# State specific actions # State specific actions
if not self . a ctive: if not CC . latA ctive:
self . LaC . reset ( ) self . LaC . reset ( )
if not CC . longActive :
self . LoC . reset ( v_pid = CS . vEgo ) self . LoC . reset ( v_pid = CS . vEgo )
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 . v_cruise_kph * CV . KPH_TO_MS ) pid_accel_limits = self . CI . get_pid_accel_limits ( self . CP , CS . vEgo , self . v_cruise_kph * CV . KPH_TO_MS )
t_since_plan = ( self . sm . frame - self . sm . rcv_frame [ ' longitudinalPlan ' ] ) * DT_CTRL t_since_plan = ( self . sm . frame - self . sm . rcv_frame [ ' longitudinalPlan ' ] ) * DT_CTRL
actuators . accel = self . LoC . update ( self . a ctive, CS , self . CP , long_plan , pid_accel_limits , t_since_plan ) actuators . accel = self . LoC . update ( CC . longA ctive, CS , self . CP , long_plan , pid_accel_limits , t_since_plan )
# Steering PID loop and lateral MPC # Steering PID loop and lateral MPC
lat_active = self . active and not CS . steerFaultTemporary and not CS . steerFaultPermanent and CS . vEgo > self . CP . minSteerSpeed
desired_curvature , desired_curvature_rate = get_lag_adjusted_curvature ( self . CP , CS . vEgo , desired_curvature , desired_curvature_rate = get_lag_adjusted_curvature ( self . CP , CS . vEgo ,
lat_plan . psis , lat_plan . psis ,
lat_plan . curvatures , lat_plan . curvatures ,
lat_plan . curvatureRates ) lat_plan . curvatureRates )
actuators . steer , actuators . steeringAngleDeg , lac_log = self . LaC . update ( lat_active , CS , self . CP , self . VM , params , self . last_actuators , actuators . steer , actuators . steeringAngleDeg , lac_log = self . LaC . update ( CC . latActive , CS , self . CP , self . VM ,
desired_curvature , desired_curvature_rate ) params , self . last_actuators , desired_curvature ,
desired_curvature_rate )
else : else :
lac_log = log . ControlsState . LateralDebugState . new_message ( ) lac_log = log . ControlsState . LateralDebugState . new_message ( )
if self . sm . rcv_frame [ ' testJoystick ' ] > 0 and self . active : if self . sm . rcv_frame [ ' testJoystick ' ] > 0 :
actuators . accel = 4.0 * clip ( self . sm [ ' testJoystick ' ] . axes [ 0 ] , - 1 , 1 ) if CC . longActive :
actuators . accel = 4.0 * clip ( self . sm [ ' testJoystick ' ] . axes [ 0 ] , - 1 , 1 )
steer = clip ( self . sm [ ' testJoystick ' ] . axes [ 1 ] , - 1 , 1 ) if CC . latActive :
# max angle is 45 for angle-based cars steer = clip ( self . sm [ ' testJoystick ' ] . axes [ 1 ] , - 1 , 1 )
actuators . steer , actuators . steeringAngleDeg = steer , steer * 45. # max angle is 45 for angle-based cars
actuators . steer , actuators . steeringAngleDeg = steer , steer * 45.
lac_log . active = Tru e lac_log . active = self . activ e
lac_log . steeringAngleDeg = CS . steeringAngleDeg lac_log . steeringAngleDeg = CS . steeringAngleDeg
lac_log . output = steer lac_log . output = actuators . steer
lac_log . saturated = abs ( steer ) > = 0.9 lac_log . saturated = abs ( actuators . steer ) > = 0.9
# Send a "steering required alert" if saturation count has reached the limit # Send a "steering required alert" if saturation count has reached the limit
if lac_log . active and lac_log . saturated and not CS . steeringPressed : if lac_log . active and lac_log . saturated and not CS . steeringPressed :
@ -540,7 +550,7 @@ class Controls:
cloudlog . error ( f " actuators. { p } not finite { actuators . to_dict ( ) } " ) cloudlog . error ( f " actuators. { p } not finite { actuators . to_dict ( ) } " )
setattr ( actuators , p , 0.0 ) setattr ( actuators , p , 0.0 )
return actuators , lac_log return CC , lac_log
def update_button_timers ( self , buttonEvents ) : def update_button_timers ( self , buttonEvents ) :
# increment timer for buttons still pressed # increment timer for buttons still pressed
@ -552,14 +562,9 @@ class Controls:
if b . type . raw in self . button_timers : if b . type . raw in self . button_timers :
self . button_timers [ b . type . raw ] = 1 if b . pressed else 0 self . button_timers [ b . type . raw ] = 1 if b . pressed else 0
def publish_logs ( self , CS , start_time , actuators , lac_log ) : def publish_logs ( self , CS , start_time , CC , lac_log ) :
""" Send actuators and hud commands to the car, send controlsstate and MPC logging """ """ Send actuators and hud commands to the car, send controlsstate and MPC logging """
CC = car . CarControl . new_message ( )
CC . enabled = self . enabled
CC . active = self . active
CC . actuators = actuators
orientation_value = self . sm [ ' liveLocationKalman ' ] . orientationNED . value orientation_value = self . sm [ ' liveLocationKalman ' ] . orientationNED . value
if len ( orientation_value ) > 2 : if len ( orientation_value ) > 2 :
CC . roll = orientation_value [ 0 ] CC . roll = orientation_value [ 0 ]
@ -580,7 +585,7 @@ class Controls:
recent_blinker = ( self . sm . frame - self . last_blinker_frame ) * DT_CTRL < 5.0 # 5s blinker cooldown recent_blinker = ( self . sm . frame - self . last_blinker_frame ) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self . is_ldw_enabled and CS . vEgo > LDW_MIN_SPEED and not recent_blinker \ ldw_allowed = self . is_ldw_enabled and CS . vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self . a ctiveand self . sm [ ' liveCalibration ' ] . calStatus == Calibration . CALIBRATED and not CC . latA ctiveand self . sm [ ' liveCalibration ' ] . calStatus == Calibration . CALIBRATED
model_v2 = self . sm [ ' modelV2 ' ] model_v2 = self . sm [ ' modelV2 ' ]
desire_prediction = model_v2 . meta . desirePrediction desire_prediction = model_v2 . meta . desirePrediction
@ -719,12 +724,12 @@ class Controls:
self . prof . checkpoint ( " State transition " ) self . prof . checkpoint ( " State transition " )
# Compute actuators (runs PID loops and lateral MPC) # Compute actuators (runs PID loops and lateral MPC)
actuators , lac_log = self . state_control ( CS ) CC , lac_log = self . state_control ( CS )
self . prof . checkpoint ( " State Control " ) self . prof . checkpoint ( " State Control " )
# Publish data # Publish data
self . publish_logs ( CS , start_time , actuators , lac_log ) self . publish_logs ( CS , start_time , CC , lac_log )
self . prof . checkpoint ( " Sent " ) self . prof . checkpoint ( " Sent " )
self . update_button_timers ( CS . buttonEvents ) self . update_button_timers ( CS . buttonEvents )
Shane Smiskol
3 years ago
committed by
GitHub