@ -25,12 +25,12 @@ JERK_THRESHOLD = 0.2
class LatControlTorque ( LatControl ) :
class LatControlTorque ( LatControl ) :
def __init__ ( self , CP , CI ) :
def __init__ ( self , CP , CI ) :
super ( ) . __init__ ( CP , CI )
super ( ) . __init__ ( CP , CI )
self . CP = CP
self . pid = PIDController ( CP . lateralTuning . torque . kp , CP . lateralTuning . torque . ki ,
self . pid = PIDController ( CP . lateralTuning . torque . kp , CP . lateralTuning . torque . ki ,
k_f = CP . lateralTuning . torque . kf , pos_limit = self . steer_max , neg_limit = - self . steer_max )
k_f = CP . lateralTuning . torque . kf , pos_limit = self . steer_max , neg_limit = - self . steer_max )
self . get_steer_feedforward = CI . get_steer_feedforward_function ( )
self . get_steer_feedforward = CI . get_steer_feedforward_function ( )
self . use_steering_angle = CP . lateralTuning . torque . useSteeringAngle
self . use_steering_angle = CP . lateralTuning . torque . useSteeringAngle
self . friction = CP . lateralTuning . torque . friction
self . friction = CP . lateralTuning . torque . friction
self . kf = CP . lateralTuning . torque . kf
def reset ( self ) :
def reset ( self ) :
super ( ) . reset ( )
super ( ) . reset ( )
@ -61,7 +61,7 @@ class LatControlTorque(LatControl):
ff = desired_lateral_accel - params . roll * ACCELERATION_DUE_TO_GRAVITY
ff = desired_lateral_accel - params . roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
# convert friction into lateral accel units for feedforward
friction_compensation = interp ( desired_lateral_jerk , [ - JERK_THRESHOLD , JERK_THRESHOLD ] , [ - self . friction , self . friction ] )
friction_compensation = interp ( desired_lateral_jerk , [ - JERK_THRESHOLD , JERK_THRESHOLD ] , [ - self . friction , self . friction ] )
ff + = friction_compensation / self . CP . lateralTuning . torque . kf
ff + = friction_compensation / self . kf
output_torque = self . pid . update ( error ,
output_torque = self . pid . update ( error ,
override = CS . steeringPressed , feedforward = ff ,
override = CS . steeringPressed , feedforward = ff ,
speed = CS . vEgo ,
speed = CS . vEgo ,