diff --git a/cereal/log.capnp b/cereal/log.capnp index e6159f4b1..54e877f2a 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -795,7 +795,6 @@ struct ControlsState @0x97ff69c53601abf1 { saturated @7 :Bool; actualLateralAccel @9 :Float32; desiredLateralAccel @10 :Float32; - desiredLateralJerk @11 :Float32; } struct LateralLQRState { diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index bc4a2bd43..ce63eb2c5 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -286,6 +286,7 @@ class CarInterfaceBase(ABC): ret.vEgoStopping = 0.5 ret.vEgoStarting = 0.5 ret.stoppingControl = True + ret.longitudinalTuning.kf = 1. ret.longitudinalTuning.kpBP = [0.] ret.longitudinalTuning.kpV = [0.] ret.longitudinalTuning.kiBP = [0.] @@ -301,6 +302,10 @@ class CarInterfaceBase(ABC): tune.init('torque') tune.torque.useSteeringAngle = use_steering_angle + tune.torque.kf = 0.95 + tune.torque.kp = 0.6 + tune.torque.ki = 0.3 + tune.torque.kd = 0.3 tune.torque.friction = params['FRICTION'] tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] tune.torque.latAccelOffset = 0.0 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 1944a5a9c..73ed3b17b 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -10,8 +10,7 @@ class LatControlPID(LatControl): super().__init__(CP, CI, dt) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - pos_limit=self.steer_max, neg_limit=-self.steer_max) - self.ff_factor = CP.lateralTuning.pid.kf + 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 update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): @@ -31,7 +30,7 @@ class LatControlPID(LatControl): else: # offset does not contribute to resistive torque - ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + ff = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2a988bd2b..db647ae50 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -27,15 +27,7 @@ MAX_LAT_JERK_UP = 2.5 # m/s^3 LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] -KP = 1.0 -KI = 0.1 -KD = 0.3 -# filter jerk and measurement rate with cutoff frequency equal ~0.5 jerk up limit -LP_FILTER_CUTOFF_HZ = 1.2 -JERK_LOOKAHEAD_SECONDS = 0.19 -JERK_GAIN = 0.3 -LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 class LatControlTorque(LatControl): def __init__(self, CP, FPCP, CI, dt): @@ -43,15 +35,14 @@ class LatControlTorque(LatControl): self.torque_params = FPCP.lateralTuning.torque self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(KP, KI, KD, rate=1/self.dt) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, + k_f=self.torque_params.kf, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) - self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) - self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) - self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt=self.dt) - self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt=self.dt) + self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt) + self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES) self.previous_measurement = 0.0 + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) # FrogPilot variables self.nnff = NeuralNetworkFeedforward(CP, self) @@ -79,24 +70,22 @@ class LatControlTorque(LatControl): curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len)) - expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] - lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2)) - raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt) - desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk) + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)) + expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] # TODO factor out lateral jerk from error to later replace it with delay independent alternative future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.lat_accel_request_buffer.append(future_desired_lateral_accel) + self.requested_lateral_accel_buffer.append(future_desired_lateral_accel) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay measurement = measured_curvature * CS.vEgo ** 2 measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) self.previous_measurement = measurement low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 - setpoint = expected_lateral_accel + setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel error = setpoint - measurement - error_lsf = error + low_speed_factor / KP * error + error_lsf = error + low_speed_factor / self.torque_params.kp * error if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite: pid_log, ff = self.nnff.compute_nnff( @@ -115,10 +104,15 @@ class LatControlTorque(LatControl): ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it + ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - output_lataccel = self.pid.update(pid_log.error, -measurement_rate, CS.vEgo, ff, freeze_integrator) + output_lataccel = self.pid.update(pid_log.error, + -measurement_rate, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True @@ -129,7 +123,6 @@ class LatControlTorque(LatControl): pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(measurement) pid_log.desiredLateralAccel = float(setpoint) - pid_log.desiredLateralJerk= float(desired_lateral_jerk) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index dfd157ef6..6563b142d 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -95,7 +95,7 @@ class LongControl: pos_p_limit = 0.0 # if params("NoPositivePResponse") else None # put parameter-based control here self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - rate=1 / DT_CTRL, + k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL, pos_p_limit=pos_p_limit) self.v_pid = 0.0 self._mode_setup() diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 55213a1b2..a076ea2e0 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -5,12 +5,13 @@ from openpilot.common.numpy_fast import clip, interp class PIDController: - def __init__(self, k_p, k_i, k_d=0., + def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100, pos_p_limit=None, neg_p_limit=None): self._k_p = k_p self._k_i = k_i self._k_d = k_d + self.k_f = k_f # feedforward gain if isinstance(self._k_p, Number): self._k_p = [[0], [self._k_p]] if isinstance(self._k_i, Number): @@ -66,7 +67,7 @@ class PIDController: elif self.neg_p_limit is not None and self.p < self.neg_p_limit: self.p = self.neg_p_limit self.d = self.k_d * error_rate - self.f = feedforward + self.f = self.k_f * feedforward if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) diff --git a/system/hardware/fan_controller.py b/system/hardware/fan_controller.py index a27f1459f..f32133f6b 100755 --- a/system/hardware/fan_controller.py +++ b/system/hardware/fan_controller.py @@ -18,7 +18,7 @@ class TiciFanController(BaseFanController): cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, rate=(1 / DT_HW)) + self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_HW)) def update(self, cur_temp: float, ignition: bool) -> int: self.controller.neg_limit = -(100 if ignition else 30)