diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index fc218bad725bf7..16c20e69b419f3 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -33,16 +33,25 @@ def apply_deadzone(angle, deadzone): class LatControl(object): def __init__(self, CP): - self.pid = PIController((CP.steerKpBP, CP.steerKpV), - (CP.steerKiBP, CP.steerKiV), - k_f=CP.steerKf, pos_limit=1.0) + + _ADJUST_REACTANCE = 1.5 + _ADJUST_INDUCTANCE = 1.5 + _ADJUST_RESISTANCE = 0.5 + + KpV = [np.interp(25.0, CP.steerKpBP, CP.steerKpV) * _ADJUST_REACTANCE] + KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _ADJUST_REACTANCE] + Kf = CP.steerKf * _ADJUST_INDUCTANCE + print(KpV, KiV, Kf) + self.pid = PIController(([0.], KpV), + ([0.], KiV), + k_f=Kf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) - self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) - self.projection_factor = 5.0 * _DT # Mutiplier for reactive component (PI) - self.accel_limit = 2.0 # Desired acceleration limit to prevent "whip steer" (resistive component) - self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward - self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward + self.smooth_factor = _ADJUST_INDUCTANCE * 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) + self.projection_factor = _ADJUST_REACTANCE * 5.0 * _DT # Mutiplier for reactive component (PI) + self.accel_limit = 2.0 / _ADJUST_RESISTANCE # Desired acceleration limit to prevent "whip steer" (resistive component) + self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward + self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward self.prev_angle_rate = 0 self.feed_forward = 0.0 self.steerActuatorDelay = CP.steerActuatorDelay