diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 1f7fb4dfa..903700d4b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -64,30 +64,30 @@ class LatControlTorque(LatControl): future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.lat_accel_request_buffer.append(future_desired_lateral_accel) + 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, 1, self.lat_accel_request_buffer_len)) + expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] + setpoint = expected_lateral_accel + error = setpoint - measurement + + 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) + gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + 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) + if not active: output_torque = 0.0 pid_log.active = False else: - 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, 1, self.lat_accel_request_buffer_len)) - expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] - setpoint = expected_lateral_accel - error = setpoint - measurement - - 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) - gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation - # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error) - 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) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 274bec8b4..7b9039180 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -cdd8ecaf03b0581d6a4df7659b916f3d22167a23 \ No newline at end of file +77951c4ccd0916b87c8dfda9faa33cd2d5d2cc11 \ No newline at end of file