diff --git a/cereal/log.capnp b/cereal/log.capnp index e756843562..cf7b2dbe03 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -918,6 +918,8 @@ struct ControlsState @0x97ff69c53601abf1 { saturated @7 :Bool; actualLateralAccel @9 :Float32; desiredLateralAccel @10 :Float32; + desiredLateralJerk @11 :Float32; + version @12 :Int32; } struct LateralLQRState { diff --git a/common/pid.py b/common/pid.py index f2ab935f45..3c7f645d67 100644 --- a/common/pid.py +++ b/common/pid.py @@ -2,7 +2,7 @@ import numpy as np from numbers import Number class PIDController: - def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): + def __init__(self, k_p, k_i, k_f=1., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): self._k_p = k_p self._k_i = k_i self._k_d = k_d @@ -14,11 +14,9 @@ class PIDController: if isinstance(self._k_d, Number): self._k_d = [[0], [self._k_d]] - self.pos_limit = pos_limit - self.neg_limit = neg_limit + self.set_limits(pos_limit, neg_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() @@ -35,10 +33,6 @@ class PIDController: def k_d(self): return np.interp(self.speed, self._k_d[0], self._k_d[1]) - @property - def error_integral(self): - return self.i/self.k_i - def reset(self): self.p = 0.0 self.i = 0.0 @@ -46,25 +40,28 @@ class PIDController: self.f = 0.0 self.control = 0 - def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): + def set_limits(self, pos_limit, neg_limit): + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): self.speed = speed + self.p = self.k_p * float(error) + self.d = self.k_d * error_rate + self.f = self.k_f * feedforward - self.p = float(error) * self.k_p - self.f = feedforward * self.k_f - self.d = error_rate * self.k_d + i_candidate = self.i if freeze_integrator else self.i + self.k_i * self.i_dt * error + u = self.p + i_candidate + self.d + self.f + u_sat = np.clip(u, self.neg_limit, self.pos_limit) - if override: - self.i -= self.i_unwind_rate * float(np.sign(self.i)) + if u == u_sat: + self.i = i_candidate else: - if not freeze_integrator: - self.i = self.i + error * self.k_i * self.i_rate - - # Clip i to prevent exceeding control limits - control_no_i = self.p + self.d + self.f - control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit) - self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) + if u > self.pos_limit and error < 0: + self.i = i_candidate + elif u < self.neg_limit and error > 0: + self.i = i_candidate control = self.p + self.i + self.d + self.f - self.control = np.clip(control, self.neg_limit, self.pos_limit) - return self.control + return self.control \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index d3bc08134b..f0bbc9d93e 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -8,6 +8,7 @@ from enum import StrEnum from typing import Any, NamedTuple from collections.abc import Callable from functools import cache +from cereal import car from opendbc.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from opendbc.car import structs @@ -50,7 +51,8 @@ class LatControlInputs(NamedTuple): aego: float -TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, bool], float] +TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, bool], float] +LateralAccelFromTorqueCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, bool], float] @cache @@ -181,14 +183,19 @@ class CarInterfaceBase(ABC): def get_steer_feedforward_function(self): return self.get_steer_feedforward_default - def torque_from_lateral_accel_linear(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning, - gravity_adjusted: bool) -> float: + def torque_from_lateral_accel_linear(self, lateral_acceleration: float, torque_params: car.CarParams.LateralTorqueTuning) -> float: # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) - return latcontrol_inputs.lateral_acceleration / float(torque_params.latAccelFactor) + return lateral_acceleration / float(torque_params.latAccelFactor) def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: return self.torque_from_lateral_accel_linear + def lateral_accel_from_torque_linear(self, torque: float, torque_params: car.CarParams.LateralTorqueTuning) -> float: + return torque * float(torque_params.latAccelFactor) + + def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType: + return self.lateral_accel_from_torque_linear + # returns a set of default params to avoid repetition in car specific params @staticmethod def get_std_params(candidate: str) -> structs.CarParams: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index dd7968b732..8397c9ecb3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -6,7 +6,7 @@ from cereal import car, log import cereal.messaging as messaging from openpilot.common.constants import CV from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL from openpilot.common.swaglog import cloudlog from opendbc.car.car_helpers import interfaces @@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl +from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose State = log.SelfdriveState.OpenpilotState @@ -35,7 +36,7 @@ class Controls: self.CI = interfaces[self.CP.carFingerprint](self.CP) - self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', + self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveDelay', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) @@ -51,11 +52,11 @@ class Controls: self.VM = VehicleModel(self.CP) self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.LaC = LatControlAngle(self.CP, self.CI) + self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL) elif self.CP.lateralTuning.which() == 'pid': - self.LaC = LatControlPID(self.CP, self.CI) + self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) elif self.CP.lateralTuning.which() == 'torque': - self.LaC = LatControlTorque(self.CP, self.CI) + self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) def update(self): self.sm.update(15) @@ -117,11 +118,11 @@ class Controls: # Reset desired curvature to current to avoid violating the limits on engage 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 + LAT_SMOOTH_SECONDS actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_controls, 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/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d67a4aa959..ebebcd0121 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,31 +1,29 @@ import numpy as np from abc import abstractmethod, ABC -from openpilot.common.realtime import DT_CTRL - class LatControl(ABC): - def __init__(self, CP, CI): - self.sat_count_rate = 1.0 * DT_CTRL + def __init__(self, CP, CI, dt): + self.dt = 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_controls, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, curvature_limited: bool, lat_delay: float): pass def reset(self): - self.sat_count = 0. + self.sat_time = 0. - def _check_saturation(self, saturated, CS, steer_limited_by_controls, curvature_limited): + 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_controls and not CS.steeringPressed: - self.sat_count += self.sat_count_rate + 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_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) \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 807161735c..62fb376fa0 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -3,16 +3,17 @@ import math from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl +# TODO This is speed dependent STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.sat_check_min_speed = 5. - self.use_steer_limited_by_controls = CP.brand == "tesla" + self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: @@ -23,9 +24,9 @@ class LatControlAngle(LatControl): angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - if self.use_steer_limited_by_controls: + if self.use_steer_limited_by_safety: # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation - angle_control_saturated = steer_limited_by_controls + angle_control_saturated = steer_limited_by_safety else: # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota) # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved @@ -33,4 +34,4 @@ class LatControlAngle(LatControl): angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des - return 0, float(angle_steers_des), angle_log + return 0, float(angle_steers_des), angle_log \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 4e4e0772fe..cfc0ac5c1d 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -6,18 +6,15 @@ from openpilot.common.pid import PIDController class LatControlPID(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.ff_factor = CP.lateralTuning.pid.kf self.get_steer_feedforward = CI.get_steer_feedforward_function() - def reset(self): - super().reset() - self.pid.reset() - - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -29,20 +26,24 @@ class LatControlPID(LatControl): pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = error if not active: - output_steer = 0.0 + output_torque = 0.0 pid_log.active = False - self.pid.reset() + else: # offset does not contribute to resistive torque - steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + ff = self.ff_factor * 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, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) - output_steer = self.pid.update(error, override=CS.steeringPressed, - feedforward=steer_feedforward, speed=CS.vEgo) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) - pid_log.output = float(output_steer) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) + pid_log.output = float(output_torque) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) - return output_steer, angle_steers_des, pid_log + return output_torque, angle_steers_des, pid_log \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 991ac3439b..16f0be9243 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,10 +1,11 @@ import math import numpy as np +from collections import deque from cereal import log from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction -from opendbc.car.interfaces import LatControlInputs from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController @@ -14,73 +15,98 @@ from openpilot.common.pid import PIDController # wheel slip, or to speed. # This controller applies torque to achieve desired lateral -# accelerations. To compensate for the low speed effects we -# use a LOW_SPEED_FACTOR in the error. Additionally, there is -# friction in the steering wheel that needs to be overcome to -# move it at all, this is compensated for too. +# accelerations. To compensate for the low speed effects the +# proportional gain is increased at low speeds by the PID controller. +# Additionally, there is friction in the steering wheel that needs +# to be overcome to move it at all, this is compensated for too. -LOW_SPEED_X = [0, 10, 20, 30] -LOW_SPEED_Y = [15, 13, 10, 5] +KP = 1.0 +KI = 0.3 +KD = 0.0 +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 +LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 +VERSION = 0 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.pid = PIDController(self.torque_params.kp, self.torque_params.ki, - k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.lateral_accel_from_torque = CI.lateral_accel_from_torque() + 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.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.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 self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction + self.update_limits() - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): + 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, lat_delay): pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = VERSION if not active: 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)) - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - - # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature - # desired_lateral_jerk = desired_curvature_rate * 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 - measurement = actual_lateral_accel + low_speed_factor * actual_curvature - gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation - torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - gravity_adjusted=False) - torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - gravity_adjusted=False) - pid_log.error = float(torque_from_setpoint - torque_from_measurement) - ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - gravity_adjusted=True) - ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + 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] + # 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 + desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay - freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 - output_torque = self.pid.update(pid_log.error, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + 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 + 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 + # 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, + 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 pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.d = float(self.pid.d) pid_log.f = float(self.pid.f) - pid_log.output = float(-output_torque) - pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(desired_lateral_accel) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) + 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 - return -output_torque, 0.0, pid_log + return -output_torque, 0.0, pid_log \ No newline at end of file