@ -24,6 +24,9 @@ from openpilot.common.pid import PIDController
LOW_SPEED_X = [ 0 , 10 , 20 , 30 ]
LOW_SPEED_X = [ 0 , 10 , 20 , 30 ]
LOW_SPEED_Y = [ 15 , 13 , 10 , 5 ]
LOW_SPEED_Y = [ 15 , 13 , 10 , 5 ]
KP = 1.0
KI = 0.3
KD = 0.0
class LatControlTorque ( LatControl ) :
class LatControlTorque ( LatControl ) :
@ -32,7 +35,7 @@ class LatControlTorque(LatControl):
self . torque_params = CP . lateralTuning . torque . as_builder ( )
self . torque_params = CP . lateralTuning . torque . as_builder ( )
self . torque_from_lateral_accel = CI . torque_from_lateral_accel ( )
self . torque_from_lateral_accel = CI . torque_from_lateral_accel ( )
self . lateral_accel_from_torque = CI . lateral_accel_from_torque ( )
self . lateral_accel_from_torque = CI . lateral_accel_from_torque ( )
self . pid = PIDController ( self . torque_params . kp , self . torque_params . ki , rate = 1 / self . dt )
self . pid = PIDController ( KP , KI , k_d = KD , rate = 1 / self . dt )
self . update_limits ( )
self . update_limits ( )
self . steering_angle_deadzone_deg = self . torque_params . steeringAngleDeadzoneDeg
self . steering_angle_deadzone_deg = self . torque_params . steeringAngleDeadzoneDeg
self . LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int ( 1 / self . dt )
self . LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int ( 1 / self . dt )
@ -76,7 +79,7 @@ class LatControlTorque(LatControl):
low_speed_factor = ( np . interp ( CS . vEgo , LOW_SPEED_X , LOW_SPEED_Y ) / max ( CS . vEgo , MIN_SPEED ) ) * * 2
low_speed_factor = ( np . interp ( CS . vEgo , LOW_SPEED_X , LOW_SPEED_Y ) / max ( CS . vEgo , MIN_SPEED ) ) * * 2
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
error = setpoint - measurement
error = setpoint - measurement
error_lsf = error + low_speed_factor / self . torque_params . kp * error
error_lsf = error + low_speed_factor / KP * error
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
pid_log . error = float ( error_lsf )
pid_log . error = float ( error_lsf )