@ -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