diff --git a/cereal/car.capnp b/cereal/car.capnp index f0522253e..056cbf8b0 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -532,6 +532,7 @@ struct CarParams { useSteeringAngle @0 :Bool; kp @1 :Float32; ki @2 :Float32; + kd @8 :Float32; friction @3 :Float32; kf @4 :Float32; steeringAngleDeadzoneDeg @5 :Float32; diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7f925f8e6..ccb7b810c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -305,6 +305,7 @@ class CarInterfaceBase(ABC): tune.torque.kf = 1.0 tune.torque.kp = 1.0 tune.torque.ki = 0.3 + tune.torque.kd = 0.0 tune.torque.friction = params['FRICTION'] tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] tune.torque.latAccelOffset = 0.0 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 52c33e8b5..b4ef85fe7 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -7,9 +7,8 @@ MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s class LatControl(ABC): def __init__(self, CP, CI, dt): self.dt = dt - self.sat_count_rate = 1.0 * self.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] @@ -20,13 +19,13 @@ class LatControl(ABC): 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.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.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_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 16215621d..35bf53480 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -3,6 +3,7 @@ import numpy as np from collections import deque from cereal import log +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol import LatControl @@ -22,6 +23,8 @@ from openpilot.frogpilot.controls.lib.neural_network_feedforward import LOW_SPEE # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. +MAX_LAT_JERK_UP = 2.5 # m/s^3 + LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] @@ -38,6 +41,8 @@ class LatControlTorque(LatControl): self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg 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) @@ -60,9 +65,10 @@ class LatControlTorque(LatControl): output_torque = 0.0 pid_log.active = False else: - actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) 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)) + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 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] @@ -71,18 +77,19 @@ class LatControlTorque(LatControl): 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 - 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_NN if frogpilot_toggles.nnff else LOW_SPEED_Y)**2 / (np.clip(CS.vEgo, MIN_SPEED, np.inf) ** 2) + 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 = lat_delay * desired_lateral_jerk + expected_lateral_accel - measurement = actual_lateral_accel error = setpoint - measurement error_lsf = error + low_speed_factor * error if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite: pid_log, ff = self.nnff.compute_nnff( - CS, VM, actual_lateral_accel, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel, + CS, VM, measurement, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel, llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles ) @@ -102,9 +109,10 @@ class LatControlTorque(LatControl): 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) + -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 @@ -113,8 +121,8 @@ class LatControlTorque(LatControl): pid_log.d = float(self.pid.d) 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(expected_lateral_accel) + pid_log.actualLateralAccel = float(measurement) + pid_log.desiredLateralAccel = float(setpoint) 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/pid.py b/selfdrive/controls/lib/pid.py index 44accf4e2..a076ea2e0 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -26,7 +26,7 @@ class PIDController: self.neg_p_limit = neg_p_limit self.i_unwind_rate = 0.3 / rate - self.i_rate = 1.0 / rate + self.i_dt = 1.0 / rate self.speed = 0.0 self.reset() @@ -61,19 +61,19 @@ class PIDController: def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): self.speed = speed - self.p = float(error) * self.k_p + self.p = self.k_p * float(error) if self.pos_p_limit is not None and self.p > self.pos_p_limit: self.p = self.pos_p_limit elif self.neg_p_limit is not None and self.p < self.neg_p_limit: self.p = self.neg_p_limit - self.f = feedforward * self.k_f - self.d = error_rate * self.k_d + self.d = self.k_d * error_rate + self.f = self.k_f * feedforward if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: if not freeze_integrator: - self.i = self.i + error * self.k_i * self.i_rate + self.i = self.i + self.k_i * self.i_dt * error # Clip i to prevent exceeding control limits control_no_i = self.p + self.d + self.f