diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ab23af7b88..ffc52cb3e2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -98,8 +98,7 @@ class Controls: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, - ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) - + ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) # set alternative experiences from parameters self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") @@ -557,15 +556,15 @@ class Controls: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL - actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, lat_plan.curvatureRates) - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, - params, self.last_actuators, desired_curvature, + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params, + self.last_actuators, desired_curvature, desired_curvature_rate, self.sm['liveLocationKalman']) else: lac_log = log.ControlsState.LateralDebugState.new_message() diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 0d137df733..785c8faa8c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,7 +1,7 @@ from abc import abstractmethod, ABC -from common.realtime import DT_CTRL from common.numpy_fast import clip +from common.realtime import DT_CTRL MIN_STEER_SPEED = 0.3 @@ -16,7 +16,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 8d09432b6a..2eee71c703 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() if CS.vEgo < MIN_STEER_SPEED or not active: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 4d2a727623..b5041eb172 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -63,7 +63,7 @@ class LatControlINDI(LatControl): self.steer_filter.x = 0. self.speed = 0. - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 813ba13aba..5dd1b20ae0 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,23 +1,23 @@ import math -from selfdrive.controls.lib.pid import PIDController -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from cereal import log +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.pid import PIDController class LatControlPID(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) 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=1.0, neg_limit=-1.0) + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() def reset(self): super().reset() self.pid.reset() - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -33,9 +33,6 @@ class LatControlPID(LatControl): pid_log.active = False self.pid.reset() else: - self.pid.pos_limit = self.steer_max - self.pid.neg_limit = -self.steer_max - # offset does not contribute to resistive torque steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 862e8b384f..a73744de74 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,9 +1,10 @@ import math -from selfdrive.controls.lib.pid import PIDController + +from cereal import log from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from cereal import log # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -12,7 +13,7 @@ from cereal import log # This controller applies torque to achieve desired lateral # accelerations. To compensate for the low speed effects we -# use a LOW_SPEED_FACTOR in the error. Additionally there is +# use a LOW_SPEED_FACTOR in the error. Additionally, there is # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. @@ -24,12 +25,10 @@ JERK_THRESHOLD = 0.2 class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) + self.CP = CP self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, - k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0) + 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.steer_max = 1.0 - self.pid.pos_limit = self.steer_max - self.pid.neg_limit = -self.steer_max self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction @@ -37,7 +36,7 @@ class LatControlTorque(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() if CS.vEgo < MIN_STEER_SPEED or not active: @@ -49,9 +48,9 @@ class LatControlTorque(LatControl): actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) else: actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo - desired_lateral_accel = desired_curvature * CS.vEgo**2 - desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2 - actual_lateral_accel = actual_curvature * CS.vEgo**2 + desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + actual_lateral_accel = actual_curvature * CS.vEgo ** 2 setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature @@ -61,7 +60,7 @@ class LatControlTorque(LatControl): ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY # convert friction into lateral accel units for feedforward friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) - ff += friction_compensation / CP.lateralTuning.torque.kf + ff += friction_compensation / self.CP.lateralTuning.torque.kf output_torque = self.pid.update(error, override=CS.steeringPressed, feedforward=ff, speed=CS.vEgo, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 0fda7789aa..f2cd282198 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,8 +1,8 @@ from cereal import car from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import CONTROL_N +from selfdrive.controls.lib.pid import PIDController from selfdrive.modeld.constants import T_IDXS LongCtrlState = car.CarControl.Actuators.LongControlState @@ -50,12 +50,13 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, return long_control_state -class LongControl(): +class LongControl: def __init__(self, CP): + self.CP = CP self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) self.v_pid = 0.0 self.last_output_accel = 0.0 @@ -64,7 +65,7 @@ class LongControl(): self.pid.reset() self.v_pid = v_pid - def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan): + def update(self, active, CS, long_plan, accel_limits, t_since_plan): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Interp control trajectory speeds = long_plan.speeds @@ -72,11 +73,11 @@ class LongControl(): v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) a_target = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) - v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_lower = 2 * (v_target_lower - v_target) / CP.longitudinalActuatorDelayLowerBound - a_target + v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_lower = 2 * (v_target_lower - v_target) / self.CP.longitudinalActuatorDelayLowerBound - a_target - v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_upper = 2 * (v_target_upper - v_target) / CP.longitudinalActuatorDelayUpperBound - a_target + v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_upper = 2 * (v_target_upper - v_target) / self.CP.longitudinalActuatorDelayUpperBound - a_target a_target = min(a_target_lower, a_target_upper) v_target_future = speeds[-1] @@ -93,7 +94,7 @@ class LongControl(): # Update state machine output_accel = self.last_output_accel - self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo, + self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, v_target, v_target_future, CS.brakePressed, CS.cruiseState.standstill) @@ -107,8 +108,8 @@ class LongControl(): # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration - prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid - deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid + deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) freeze_integrator = prevent_overshoot error = self.v_pid - CS.vEgo @@ -121,8 +122,8 @@ class LongControl(): # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped - if not CS.standstill or output_accel > CP.stopAccel: - output_accel -= CP.stoppingDecelRate * DT_CTRL + if not CS.standstill or output_accel > self.CP.stopAccel: + output_accel -= self.CP.stoppingDecelRate * DT_CTRL output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) self.reset(CS.vEgo)