diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index c6658bdc7..df773889a 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -14,11 +14,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants LAT_PLAN_MIN_IDX = 5 LATERAL_LAG_MOD = 0.0 # seconds, modifies how far in the future we look ahead for the lateral plan -# from selfdrive/controls/lib/latcontrol_torque.py -KP = 0.8 -KI = 0.15 -INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] -KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] +KP = 1.0 +KI = 0.3 def get_predicted_lateral_jerk(lat_accels, t_diffs): @@ -61,9 +58,10 @@ class LatControlTorqueExtBase: self.lookahead_lateral_jerk: float = 0.0 self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space() + self.torque_params = lac_torque.torque_params self._ff = 0.0 - self._pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI) + self._pid = PIDController(KP, KI) self._pid_log = None self._setpoint = 0.0 self._measurement = 0.0 diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index 2db88299c..1738a11e4 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -75,14 +75,14 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): def update_feedforward_torque_space(self, CS): torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo), - self.lac_torque.torque_params, gravity_adjusted=False) + self.torque_params, gravity_adjusted=False) torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo), - self.lac_torque.torque_params, gravity_adjusted=False) + self.torque_params, gravity_adjusted=False) self._pid_log.error = float(torque_from_setpoint - torque_from_measurement) self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation, - CS.vEgo, CS.aEgo), self.lac_torque.torque_params, gravity_adjusted=True) + CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True) self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone, - FRICTION_THRESHOLD, self.lac_torque.torque_params) + FRICTION_THRESHOLD, self.torque_params) def update_output_torque(self, CS): freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 @@ -159,6 +159,6 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): # apply friction override for cars with low NN friction response if self.model.friction_override: - self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.lac_torque.torque_params) + self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) self.update_output_torque(CS)