diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 0458178ee3..10170bf3ef 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -155,6 +155,7 @@ class CarInterfaceBase(ABC): # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque + # TODO: set PID gains? ret.minSteerSpeed = 0. ret.wheelSpeedFactor = 1.0 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index affb227ee8..ff69e5de94 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -14,6 +14,16 @@ class CarInterface(CarInterfaceBase): def get_pid_accel_limits(CP, current_speed, cruise_speed): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + @staticmethod + def get_angle_feedforward(desired_angle, v_ego): + return desired_angle + + def get_steer_feedforward_function(self): + if self.CP.steerControlType == car.CarParams.SteerControlType.torque: + return CarInterfaceBase.get_steer_feedforward_default + else: + return self.get_angle_feedforward + @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "toyota" @@ -212,6 +222,13 @@ class CarInterface(CarInterfaceBase): if ret.steerControlType == car.CarParams.SteerControlType.angle: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kiBP = [0.0] + ret.lateralTuning.pid.kpBP = [0.0] + ret.lateralTuning.pid.kpV = [0.5] + ret.lateralTuning.pid.kiV = [0.1] + ret.lateralTuning.pid.kf = 1.0 + # we can't use the fingerprint to detect this reliably, since # the EV gas pedal signal can take a couple seconds to appear if candidate in EV_HYBRID_CAR: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2bb2fab15c..fcebeae8d3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -157,9 +157,9 @@ class Controls: self.VM = VehicleModel(self.CP) self.LaC: LatControl - if self.CP.steerControlType == SteerControlType.angle: - self.LaC = LatControlAngle(self.CP, self.CI) - elif self.CP.lateralTuning.which() == 'pid': + # if self.CP.steerControlType == SteerControlType.angle: + # self.LaC = LatControlAngle(self.CP, self.CI) + if self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP, self.CI) @@ -619,8 +619,7 @@ class Controls: lac_log.saturated = abs(actuators.steer) >= 0.9 # Send a "steering required alert" if saturation count has reached the limit - if (lac_log.active and not CS.steeringPressed and self.CP.steerControlType == SteerControlType.torque and - self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode): + if (lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode): undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 turning = abs(lac_log.desiredLateralAccel) > 1.0 good_speed = CS.vEgo > 5 @@ -774,8 +773,8 @@ class Controls: lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: controlsState.lateralControlState.debugState = lac_log - elif self.CP.steerControlType == SteerControlType.angle: - controlsState.lateralControlState.angleState = lac_log + # elif self.CP.steerControlType == SteerControlType.angle: + # controlsState.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': controlsState.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 982d506d7c..fabb41d99c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -8,8 +8,9 @@ MIN_STEER_SPEED = 0.0 class LatControl(ABC): def __init__(self, CP, CI): + self.CP = CP self.sat_count_rate = 1.0 * DT_CTRL - self.sat_limit = CP.steerLimitTimer + self.sat_limit = self.CP.steerLimitTimer self.sat_count = 0. # we define the steer torque scale as [-1.0...1.0] diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 6bd678073e..0fbdd98d1e 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,16 +1,23 @@ import math -from cereal import log +from cereal import car, log from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from selfdrive.controls.lib.pid import PIDController +SteerControlType = car.CarParams.SteerControlType +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees + class LatControlPID(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) + kargs = {} + if self.CP.steerControlType == SteerControlType.torque: + kargs.update({"pos_limit": self.steer_max, "neg_limit": -self.steer_max}) + self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + k_f=CP.lateralTuning.pid.kf, **kargs) self.get_steer_feedforward = CI.get_steer_feedforward_function() def reset(self): @@ -43,6 +50,15 @@ class LatControlPID(LatControl): pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer - pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited) + + if self.CP.steerControlType != SteerControlType.torque: + angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD + pid_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited) + else: + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited) + + if self.CP.steerControlType != SteerControlType.torque: + angle_steers_des = output_steer + output_steer = 0 return output_steer, angle_steers_des, pid_log