mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-10-01 03:53:41 +08:00
Merge branch 'upstream/openpilot/master' into sync-20251114
# Conflicts: # .github/workflows/ci_weekly_run.yaml # .github/workflows/raylib_ui_preview.yaml # .github/workflows/tests.yaml # .gitmodules # README.md # SConstruct # common/api.py # common/params_keys.h # docs/CARS.md # msgq_repo # opendbc_repo # panda # selfdrive/car/tests/test_car_interfaces.py # selfdrive/controls/controlsd.py # selfdrive/controls/lib/latcontrol.py # selfdrive/controls/lib/latcontrol_angle.py # selfdrive/controls/lib/latcontrol_pid.py # selfdrive/controls/lib/latcontrol_torque.py # selfdrive/controls/tests/test_latcontrol.py # selfdrive/monitoring/helpers.py # selfdrive/ui/SConscript # selfdrive/ui/main.cc # selfdrive/ui/qt/body.h # selfdrive/ui/qt/home.cc # selfdrive/ui/qt/home.h # selfdrive/ui/qt/network/networking.cc # selfdrive/ui/qt/network/networking.h # selfdrive/ui/qt/network/wifi_manager.cc # selfdrive/ui/qt/offroad/developer_panel.cc # selfdrive/ui/qt/offroad/developer_panel.h # selfdrive/ui/qt/offroad/experimental_mode.cc # selfdrive/ui/qt/offroad/firehose.cc # selfdrive/ui/qt/offroad/firehose.h # selfdrive/ui/qt/offroad/onboarding.cc # selfdrive/ui/qt/offroad/onboarding.h # selfdrive/ui/qt/offroad/settings.cc # selfdrive/ui/qt/offroad/settings.h # selfdrive/ui/qt/offroad/software_settings.cc # selfdrive/ui/qt/onroad/alerts.cc # selfdrive/ui/qt/onroad/annotated_camera.h # selfdrive/ui/qt/onroad/buttons.cc # selfdrive/ui/qt/onroad/buttons.h # selfdrive/ui/qt/onroad/driver_monitoring.cc # selfdrive/ui/qt/onroad/hud.cc # selfdrive/ui/qt/onroad/hud.h # selfdrive/ui/qt/onroad/model.cc # selfdrive/ui/qt/onroad/model.h # selfdrive/ui/qt/onroad/onroad_home.cc # selfdrive/ui/qt/onroad/onroad_home.h # selfdrive/ui/qt/request_repeater.h # selfdrive/ui/qt/sidebar.cc # selfdrive/ui/qt/sidebar.h # selfdrive/ui/qt/util.cc # selfdrive/ui/qt/widgets/cameraview.h # selfdrive/ui/qt/widgets/controls.cc # selfdrive/ui/qt/widgets/controls.h # selfdrive/ui/qt/widgets/input.cc # selfdrive/ui/qt/widgets/input.h # selfdrive/ui/qt/widgets/prime.cc # selfdrive/ui/qt/widgets/prime.h # selfdrive/ui/qt/widgets/ssh_keys.h # selfdrive/ui/qt/widgets/toggle.h # selfdrive/ui/qt/widgets/wifi.cc # selfdrive/ui/qt/widgets/wifi.h # selfdrive/ui/qt/window.cc # selfdrive/ui/qt/window.h # selfdrive/ui/tests/cycle_offroad_alerts.py # selfdrive/ui/tests/test_ui/run.py # selfdrive/ui/translations/main_ar.ts # selfdrive/ui/translations/main_de.ts # selfdrive/ui/translations/main_es.ts # selfdrive/ui/translations/main_fr.ts # selfdrive/ui/translations/main_ja.ts # selfdrive/ui/translations/main_ko.ts # selfdrive/ui/translations/main_nl.ts # selfdrive/ui/translations/main_pl.ts # selfdrive/ui/translations/main_pt-BR.ts # selfdrive/ui/translations/main_th.ts # selfdrive/ui/translations/main_tr.ts # selfdrive/ui/translations/main_zh-CHS.ts # selfdrive/ui/translations/main_zh-CHT.ts # selfdrive/ui/ui.cc # selfdrive/ui/ui.h # system/manager/build.py # system/version.py
This commit is contained in:
Jason Wen
@@ -8,7 +8,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, DT_CTRL, Priority, Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
@@ -19,6 +19,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
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
@@ -45,7 +46,7 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
|
||||
self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
|
||||
|
||||
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'liveDelay'] + self.sm_services_ext,
|
||||
poll='selfdriveState')
|
||||
@@ -62,11 +63,11 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
self.VM = VehicleModel(self.CP)
|
||||
self.LaC: LatControl
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
|
||||
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
|
||||
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI)
|
||||
self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
|
||||
def update(self):
|
||||
self.sm.update(15)
|
||||
@@ -139,11 +140,12 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
# 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_safety, self.desired_curvature,
|
||||
self.calibrated_pose, curvature_limited) # TODO what if not available
|
||||
self.calibrated_pose, curvature_limited, lat_delay)
|
||||
actuators.torque = float(steer)
|
||||
actuators.steeringAngleDeg = float(steeringAngleDeg)
|
||||
# Ensure no NaNs/Infs
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1,31 +1,31 @@
|
||||
import numpy as np
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.locationd.helpers import Pose
|
||||
|
||||
|
||||
class LatControl(ABC):
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
self.sat_count_rate = 1.0 * DT_CTRL
|
||||
def __init__(self, CP, CP_SP, 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_safety, desired_curvature, calibrated_pose, curvature_limited):
|
||||
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, calibrated_pose: Pose,
|
||||
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_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)
|
||||
|
||||
@@ -8,12 +8,12 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
|
||||
class LatControlAngle(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, CI, dt)
|
||||
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, calibrated_pose, curvature_limited):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if not active:
|
||||
|
||||
@@ -6,14 +6,15 @@ from openpilot.common.pid import PIDController
|
||||
|
||||
|
||||
class LatControlPID(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, 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 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, calibrated_pose, curvature_limited, lat_delay):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
@@ -30,7 +31,7 @@ class LatControlPID(LatControl):
|
||||
|
||||
else:
|
||||
# offset does not contribute to resistive torque
|
||||
ff = 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,
|
||||
|
||||
@@ -1,9 +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 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
|
||||
|
||||
@@ -15,25 +17,34 @@ from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import La
|
||||
# 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, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, 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([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)
|
||||
|
||||
self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
|
||||
|
||||
@@ -47,57 +58,68 @@ class LatControlTorque(LatControl):
|
||||
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, calibrated_pose, curvature_limited):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
|
||||
# Override torque params from extension
|
||||
if self.extension.update_override_torque_params(self.torque_params):
|
||||
self.update_limits()
|
||||
|
||||
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
|
||||
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
|
||||
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
|
||||
|
||||
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(setpoint - measurement)
|
||||
ff = gravity_adjusted_lateral_accel
|
||||
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(desired_lateral_accel - actual_lateral_accel, 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,
|
||||
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)
|
||||
|
||||
# Lateral acceleration torque controller extension updates
|
||||
# Overrides pid_log.error and output_torque
|
||||
pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
|
||||
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
|
||||
desired_curvature, actual_curvature, steer_limited_by_safety, output_torque)
|
||||
future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
|
||||
desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
|
||||
|
||||
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) # TODO: log lat accel?
|
||||
pid_log.actualLateralAccel = float(actual_lateral_accel)
|
||||
pid_log.desiredLateralAccel = float(desired_lateral_accel)
|
||||
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
|
||||
|
||||
@@ -54,7 +54,7 @@ class LongControl:
|
||||
self.long_control_state = LongCtrlState.off
|
||||
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
|
||||
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
|
||||
k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
|
||||
rate=1 / DT_CTRL)
|
||||
self.last_output_accel = 0.0
|
||||
|
||||
def reset(self):
|
||||
|
||||
@@ -7,6 +7,7 @@ from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.nissan.values import CAR as NISSAN
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car.helpers import convert_to_capnp
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
@@ -29,7 +30,7 @@ class TestLatControl:
|
||||
CP_SP = convert_to_capnp(CP_SP)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
controller = controller(CP.as_reader(), CP_SP.as_reader(), CI)
|
||||
controller = controller(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
@@ -42,13 +43,13 @@ class TestLatControl:
|
||||
|
||||
# Saturate for curvature limited and controller limited
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True)
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
|
||||
assert lac_log.saturated
|
||||
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False)
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
|
||||
assert not lac_log.saturated
|
||||
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False)
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
|
||||
assert lac_log.saturated
|
||||
|
||||
Reference in New Issue
Block a user