diff --git a/common/pid.py b/common/pid.py index 99142280ca4839..ff5084781f7e4a 100644 --- a/common/pid.py +++ b/common/pid.py @@ -16,7 +16,7 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, self.set_limits(pos_limit, neg_limit) - self.i_rate = 1.0 / rate + self.i_dt = 1.0 / rate self.speed = 0.0 self.reset() @@ -46,12 +46,12 @@ def set_limits(self, pos_limit, neg_limit): def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): self.speed = speed - self.p = float(error) * self.k_p - self.f = feedforward * self.k_f - self.d = error_rate * self.k_d + self.p = self.k_p * float(error) + self.d = self.k_d * error_rate + self.f = self.k_f * feedforward if not freeze_integrator: - i = self.i + error * self.k_i * self.i_rate + i = self.i + self.k_i * self.i_dt * error # Don't allow windup if already clipping test_control = self.p + i + self.d + self.f diff --git a/opendbc_repo b/opendbc_repo index aa3d32a63b7d05..d192e1e501d92d 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit aa3d32a63b7d05e69777fa90147192220abde8a3 +Subproject commit d192e1e501d92d9d269e608507b847adbb27ba7b diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 029d16e59e7f49..592a78909fe352 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -36,7 +36,7 @@ def __init__(self) -> None: self.CI = interfaces[self.CP.carFingerprint](self.CP) self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', + 'liveCalibration', 'liveDelay', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) @@ -118,10 +118,12 @@ def state_control(self): new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) + lat_delay = self.sm["liveDelay"].lateralDelay + actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_safety, self.desired_curvature, - curvature_limited) # TODO what if not available + curvature_limited, lat_delay) actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index e28fa3021c49bd..bf6dd04f603a2f 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -22,7 +22,7 @@ def smooth_value(val, prev_val, tau, dt=DT_MDL): alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 return alpha * val + (1 - alpha) * prev_val -def clip_curvature(v_ego, prev_curvature, new_curvature, roll): +def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, bool]: # This function respects ISO lateral jerk and acceleration limits + a max curvature v_ego = max(v_ego, MIN_SPEED) max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 2a8b873e2e592a..80d33d9751d693 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -5,27 +5,28 @@ class LatControl(ABC): - def __init__(self, CP, CI): - self.sat_count_rate = 1.0 * DT_CTRL + def __init__(self, CP, CI, dt=DT_CTRL): + self.dt = dt + self.sat_time_dt = 1.0 * dt self.sat_limit = CP.steerLimitTimer - self.sat_count = 0. + self.sat_time = 0. self.sat_check_min_speed = 10. # we define the steer torque scale as [-1.0...1.0] self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): pass def reset(self): - self.sat_count = 0. + self.sat_time = 0. def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): # Saturated only if control output is not being limited by car torque/angle rate limits if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: - self.sat_count += self.sat_count_rate + self.sat_time += self.sat_time_dt else: - self.sat_count -= self.sat_count_rate - self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit) - return self.sat_count > (self.sat_limit - 1e-3) + self.sat_time -= self.sat_time_dt + self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit) + return self.sat_time > (self.sat_limit - 1e-3) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index ac35151487c1c5..a8c71ad412ebbd 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 00a083509fb756..6b318c78e53375 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,7 +13,7 @@ def __init__(self, CP, CI): 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): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 5a2814e0894879..013e990702c870 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,10 +1,13 @@ +from collections import deque import math import numpy as np from cereal import log from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction +from opendbc.car.tests.test_lateral_limits import MAX_LAT_JERK_UP from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.pid import PIDController # At higher speeds (25+mph) we can assume: @@ -21,17 +24,23 @@ LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] - class LatControlTorque(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, - k_f=self.torque_params.kf) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg + self.lataccel_request_buffer_size = int(1 / self.dt) + self.requested_lateral_accel_buffer = deque([0.] * self.lataccel_request_buffer_size , maxlen=self.lataccel_request_buffer_size) + self.error_pre = 0.0 + self.measurement_pre = 0.0 + self.gravity_adjusted_lateral_accel_pre = 0.0 + self.error_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) + self.ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) + self.jerk_ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -43,7 +52,7 @@ def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -53,27 +62,41 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + delay_frames = min(max(int(lat_delay / self.dt), 1), self.lataccel_request_buffer_size - 1) + plan_future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + self.requested_lateral_accel_buffer.append(plan_future_desired_lateral_accel) + current_expected_lateral_accel = self.requested_lateral_accel_buffer[-(delay_frames + 1)] + current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2) actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 - setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + # pid error calculated as difference between expected and measured lateral acceleration + setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation - + gravity_adjusted_lateral_accel = plan_future_desired_lateral_accel - roll_compensation + error = float(setpoint_expected - measurement) + meas_rate = (measurement - self.measurement_pre) / self.dt + meas_rate_filtered = self.error_rate_filter.update(meas_rate) + jerk_ff = (gravity_adjusted_lateral_accel - setpoint_expected) / (delay_frames * self.dt) + jerk_ff_filtered = self.jerk_ff_filter.update(jerk_ff) + self.error_pre = error + self.measurement_pre = measurement + self.gravity_adjusted_lateral_accel_pre = gravity_adjusted_lateral_accel # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(setpoint - measurement) + pid_log.error = float(error) ff = gravity_adjusted_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += self.torque_params.friction * np.tanh(2.5 * jerk_ff_filtered + 0.5*error) # get_friction(jerk_ff_filtered, 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, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + -meas_rate_filtered, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator,) + #error_expected=error_expected) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True @@ -83,7 +106,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(desired_lateral_accel) + pid_log.desiredLateralAccel = float(plan_future_desired_lateral_accel) 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/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0ce06dc9962765..8fff214602fc26 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -32,13 +32,13 @@ def test_saturation(self, car_name, controller): # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True, 0) assert lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False, 0) assert not lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False, 0) assert lac_log.saturated diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a833fadb94813f..4a57fd3ada98ba 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -afcab1abb62b9d5678342956cced4712f44e909e \ No newline at end of file +4e2926cc457a057980633163eabe062e421e9ff6 \ No newline at end of file