diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 636f28956..bf43487da 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -30,7 +30,7 @@ class Car: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState', 'liveCalibration', 'onroadEvents', 'frogpilotOnroadEvents', 'frogpilotPlan']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'liveCalibration', 'onroadEvents', 'frogpilotOnroadEvents', 'frogpilotPlan']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'frogpilotCarState']) self.can_rcv_cum_timeout_counter = 0 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index eb45d0191..fa6bf76b3 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -28,10 +28,8 @@ INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] LP_FILTER_CUTOFF_HZ = 1.2 -JERK_LOOKAHEAD_SECONDS = 0.19 -JERK_GAIN = 0.3 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 -VERSION = 1 +VERSION = 0 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -42,12 +40,10 @@ class LatControlTorque(LatControl): self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, 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), self.dt) - self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) self.previous_measurement = 0.0 + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -73,18 +69,17 @@ class LatControlTorque(LatControl): 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) + # 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) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation - setpoint = expected_lateral_accel + 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 + setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel error = setpoint - measurement # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly @@ -92,10 +87,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 diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index bdddb0000..7cc0d72dc 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ