@ -1,8 +1,13 @@
from openpilot . common . numpy_fast import clip , interp
from openpilot . common . numpy_fast import clip , interp
from opendbc . can . packer import CANPacker
from opendbc . can . packer import CANPacker
from openpilot . selfdrive . car import apply_driver_steer_torque_limits
from openpilot . selfdrive . car import apply_driver_steer_torque_limits , common_fault_avoidance
from openpilot . selfdrive . car . subaru import subarucan
from openpilot . selfdrive . car . subaru import subarucan
from openpilot . selfdrive . car . subaru . values import DBC , GLOBAL_GEN2 , PREGLOBAL_CARS , HYBRID_CARS , CanBus , CarControllerParams , SubaruFlags
from openpilot . selfdrive . car . subaru . values import DBC , GLOBAL_GEN2 , PREGLOBAL_CARS , HYBRID_CARS , STEER_RATE_LIMITED , CanBus , CarControllerParams , SubaruFlags
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
# involves the total steering angle change rather than rate, but these limits work well for now
MAX_STEER_RATE = 25 # deg/s
MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut
class CarController :
class CarController :
@ -12,6 +17,7 @@ class CarController:
self . frame = 0
self . frame = 0
self . cruise_button_prev = 0
self . cruise_button_prev = 0
self . steer_rate_counter = 0
self . p = CarControllerParams ( CP )
self . p = CarControllerParams ( CP )
self . packer = CANPacker ( DBC [ CP . carFingerprint ] [ ' pt ' ] )
self . packer = CANPacker ( DBC [ CP . carFingerprint ] [ ' pt ' ] )
@ -38,7 +44,15 @@ class CarController:
if self . CP . carFingerprint in PREGLOBAL_CARS :
if self . CP . carFingerprint in PREGLOBAL_CARS :
can_sends . append ( subarucan . create_preglobal_steering_control ( self . packer , self . frame / / self . p . STEER_STEP , apply_steer , CC . latActive ) )
can_sends . append ( subarucan . create_preglobal_steering_control ( self . packer , self . frame / / self . p . STEER_STEP , apply_steer , CC . latActive ) )
else :
else :
can_sends . append ( subarucan . create_steering_control ( self . packer , apply_steer , CC . latActive ) )
apply_steer_req = CC . latActive
if self . CP . carFingerprint in STEER_RATE_LIMITED :
# Steering rate fault prevention
self . steer_rate_counter , apply_steer_req = \
common_fault_avoidance ( abs ( CS . out . steeringRateDeg ) > MAX_STEER_RATE , apply_steer_req ,
self . steer_rate_counter , MAX_STEER_RATE_FRAMES )
can_sends . append ( subarucan . create_steering_control ( self . packer , apply_steer , apply_steer_req ) )
self . apply_steer_last = apply_steer
self . apply_steer_last = apply_steer