base LatControl class (#21967)
* base LatControl class, move sat check out of pid.py clean up clean up * fix * global variable for min control speed * nicer name * unify latcontrol class init arguments * add to release files * saturated if close to limit * move angle mode saturation checks into class * check_saturation function takes in current saturated status undo * apply latcontrol_angle's active checking to all controllers * clean up * move those back * make abstract baseclass * add test for saturation * keep clip * update ref * fix static analysis Co-authored-by: Willem Melching <willem.melching@gmail.com>pull/23243/head
Shane Smiskol
GitHub
parent
1d4191956b
commit
9de8f8cd8c
|
|
@ -238,6 +238,7 @@ selfdrive/controls/lib/latcontrol_angle.py
|
|||
selfdrive/controls/lib/latcontrol_indi.py
|
||||
selfdrive/controls/lib/latcontrol_lqr.py
|
||||
selfdrive/controls/lib/latcontrol_pid.py
|
||||
selfdrive/controls/lib/latcontrol.py
|
||||
selfdrive/controls/lib/lateral_planner.py
|
||||
selfdrive/controls/lib/longcontrol.py
|
||||
selfdrive/controls/lib/longitudinal_planner.py
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this
|
||||
ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
ret.steerLimitTimer = 0.8
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerRateCost = 1.0
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
tire_stiffness_factor = 0.5328
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.carName = "nissan"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
|
||||
|
||||
ret.steerLimitAlert = False
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerRateCost = 0.5
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.openpilotLongitudinalControl = False
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
|
||||
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerRateCost = 0.5
|
||||
|
||||
|
|
|
|||
|
|
@ -31,8 +31,6 @@ from selfdrive.manager.process_config import managed_processes
|
|||
SOFT_DISABLE_TIME = 3 # seconds
|
||||
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
|
||||
LANE_DEPARTURE_THRESHOLD = 0.1
|
||||
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
|
||||
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
SIMULATION = "SIMULATION" in os.environ
|
||||
|
|
@ -128,13 +126,13 @@ class Controls:
|
|||
self.VM = VehicleModel(self.CP)
|
||||
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP)
|
||||
self.LaC = LatControlAngle(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'indi':
|
||||
self.LaC = LatControlINDI(self.CP)
|
||||
self.LaC = LatControlINDI(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'lqr':
|
||||
self.LaC = LatControlLQR(self.CP)
|
||||
self.LaC = LatControlLQR(self.CP, self.CI)
|
||||
|
||||
self.initialized = False
|
||||
self.state = State.disabled
|
||||
|
|
@ -148,7 +146,6 @@ class Controls:
|
|||
self.cruise_mismatch_counter = 0
|
||||
self.can_rcv_error_counter = 0
|
||||
self.last_blinker_frame = 0
|
||||
self.saturated_count = 0
|
||||
self.distance_traveled = 0
|
||||
self.last_functional_fan_frame = 0
|
||||
self.events_prev = []
|
||||
|
|
@ -518,19 +515,8 @@ class Controls:
|
|||
lac_log.output = steer
|
||||
lac_log.saturated = abs(steer) >= 0.9
|
||||
|
||||
# Check for difference between desired angle and angle for angle based control
|
||||
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
|
||||
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
|
||||
|
||||
if angle_control_saturated and not CS.steeringPressed and self.active:
|
||||
self.saturated_count += 1
|
||||
else:
|
||||
self.saturated_count = 0
|
||||
|
||||
# Send a "steering required alert" if saturation count has reached the limit
|
||||
if (lac_log.saturated and not CS.steeringPressed) or \
|
||||
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
|
||||
|
||||
if lac_log.active and lac_log.saturated and not CS.steeringPressed:
|
||||
dpath_points = lat_plan.dPathPoints
|
||||
if len(dpath_points):
|
||||
# Check if we deviated from the path
|
||||
|
|
|
|||
|
|
@ -0,0 +1,28 @@
|
|||
from abc import abstractmethod, ABC
|
||||
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import clip
|
||||
|
||||
MIN_STEER_SPEED = 0.3
|
||||
|
||||
|
||||
class LatControl(ABC):
|
||||
def __init__(self, CP, CI):
|
||||
self.sat_count_rate = 1.0 * DT_CTRL
|
||||
self.sat_limit = CP.steerLimitTimer
|
||||
self.sat_count = 0.
|
||||
|
||||
@abstractmethod
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
self.sat_count = 0.
|
||||
|
||||
def _check_saturation(self, saturated, CS):
|
||||
if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed:
|
||||
self.sat_count += self.sat_count_rate
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
self.sat_count = clip(self.sat_count, 0.0, 1.0)
|
||||
return self.sat_count > self.sat_limit
|
||||
|
|
@ -1,19 +1,16 @@
|
|||
import math
|
||||
|
||||
from cereal import log
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
|
||||
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
|
||||
class LatControlAngle():
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
pass
|
||||
|
||||
class LatControlAngle(LatControl):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if CS.vEgo < 0.3 or not active:
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
angle_log.active = False
|
||||
angle_steers_des = float(CS.steeringAngleDeg)
|
||||
else:
|
||||
|
|
@ -21,8 +18,8 @@ class LatControlAngle():
|
|||
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
|
||||
angle_steers_des += params.angleOffsetDeg
|
||||
|
||||
angle_log.saturated = False
|
||||
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
|
||||
angle_log.saturated = self._check_saturation(angle_control_saturated, CS)
|
||||
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
|
||||
return 0, float(angle_steers_des), angle_log
|
||||
|
|
|
|||
|
|
@ -8,10 +8,12 @@ from common.realtime import DT_CTRL
|
|||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.car.toyota.values import CarControllerParams
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
|
||||
|
||||
class LatControlINDI():
|
||||
def __init__(self, CP):
|
||||
class LatControlINDI(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
self.angle_steers_des = 0.
|
||||
|
||||
A = np.array([[1.0, DT_CTRL, 0.0],
|
||||
|
|
@ -42,8 +44,6 @@ class LatControlINDI():
|
|||
self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
|
||||
self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)
|
||||
|
||||
self.sat_count_rate = 1.0 * DT_CTRL
|
||||
self.sat_limit = CP.steerLimitTimer
|
||||
self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
|
||||
|
||||
self.reset()
|
||||
|
|
@ -65,24 +65,12 @@ class LatControlINDI():
|
|||
return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self.steer_filter.x = 0.
|
||||
self.output_steer = 0.
|
||||
self.sat_count = 0.
|
||||
self.speed = 0.
|
||||
|
||||
def _check_saturation(self, control, check_saturation, limit):
|
||||
saturated = abs(control) == limit
|
||||
|
||||
if saturated and check_saturation:
|
||||
self.sat_count += self.sat_count_rate
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
|
||||
self.sat_count = clip(self.sat_count, 0.0, 1.0)
|
||||
|
||||
return self.sat_count > self.sat_limit
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
self.speed = CS.vEgo
|
||||
# Update Kalman filter
|
||||
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
|
||||
|
|
@ -93,14 +81,14 @@ class LatControlINDI():
|
|||
indi_log.steeringRateDeg = math.degrees(self.x[1])
|
||||
indi_log.steeringAccelDeg = math.degrees(self.x[2])
|
||||
|
||||
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
|
||||
steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)
|
||||
steers_des += math.radians(params.angleOffsetDeg)
|
||||
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
|
||||
|
||||
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
|
||||
rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)
|
||||
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
|
||||
|
||||
if CS.vEgo < 0.3 or not active:
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
indi_log.active = False
|
||||
self.output_steer = 0.0
|
||||
self.steer_filter.x = 0.0
|
||||
|
|
@ -142,8 +130,6 @@ class LatControlINDI():
|
|||
indi_log.delayedOutput = float(self.steer_filter.x)
|
||||
indi_log.delta = float(delta_u)
|
||||
indi_log.output = float(self.output_steer)
|
||||
|
||||
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
|
||||
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
|
||||
indi_log.saturated = self._check_saturation(steers_max - abs(self.output_steer) < 1e-3, CS)
|
||||
|
||||
return float(self.output_steer), float(steers_des), indi_log
|
||||
|
|
|
|||
|
|
@ -5,10 +5,12 @@ from common.numpy_fast import clip
|
|||
from common.realtime import DT_CTRL
|
||||
from cereal import log
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
|
||||
|
||||
class LatControlLQR():
|
||||
def __init__(self, CP):
|
||||
class LatControlLQR(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
self.scale = CP.lateralTuning.lqr.scale
|
||||
self.ki = CP.lateralTuning.lqr.ki
|
||||
|
||||
|
|
@ -23,26 +25,11 @@ class LatControlLQR():
|
|||
self.i_unwind_rate = 0.3 * DT_CTRL
|
||||
self.i_rate = 1.0 * DT_CTRL
|
||||
|
||||
self.sat_count_rate = 1.0 * DT_CTRL
|
||||
self.sat_limit = CP.steerLimitTimer
|
||||
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self.i_lqr = 0.0
|
||||
self.sat_count = 0.0
|
||||
|
||||
def _check_saturation(self, control, check_saturation, limit):
|
||||
saturated = abs(control) == limit
|
||||
|
||||
if saturated and check_saturation:
|
||||
self.sat_count += self.sat_count_rate
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
|
||||
self.sat_count = clip(self.sat_count, 0.0, 1.0)
|
||||
|
||||
return self.sat_count > self.sat_limit
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
lqr_log = log.ControlsState.LateralLQRState.new_message()
|
||||
|
|
@ -64,7 +51,7 @@ class LatControlLQR():
|
|||
e = steering_angle_no_offset - angle_steers_k
|
||||
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
|
||||
|
||||
if CS.vEgo < 0.3 or not active:
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
lqr_log.active = False
|
||||
lqr_output = 0.
|
||||
output_steer = 0.
|
||||
|
|
@ -91,12 +78,9 @@ class LatControlLQR():
|
|||
output_steer = lqr_output + self.i_lqr
|
||||
output_steer = clip(output_steer, -steers_max, steers_max)
|
||||
|
||||
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
|
||||
saturated = self._check_saturation(output_steer, check_saturation, steers_max)
|
||||
|
||||
lqr_log.steeringAngleDeg = angle_steers_k
|
||||
lqr_log.i = self.i_lqr
|
||||
lqr_log.output = output_steer
|
||||
lqr_log.lqrOutput = lqr_output
|
||||
lqr_log.saturated = saturated
|
||||
lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
||||
return output_steer, desired_angle, lqr_log
|
||||
|
|
|
|||
|
|
@ -2,18 +2,20 @@ import math
|
|||
|
||||
from selfdrive.controls.lib.pid import PIController
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
from cereal import log
|
||||
|
||||
|
||||
class LatControlPID():
|
||||
class LatControlPID(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
|
||||
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
|
||||
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
|
||||
sat_limit=CP.steerLimitTimer)
|
||||
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
|
||||
self.get_steer_feedforward = CI.get_steer_feedforward_function()
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
|
|
@ -26,7 +28,7 @@ class LatControlPID():
|
|||
|
||||
pid_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
|
||||
if CS.vEgo < 0.3 or not active:
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
output_steer = 0.0
|
||||
pid_log.active = False
|
||||
self.pid.reset()
|
||||
|
|
@ -40,14 +42,13 @@ class LatControlPID():
|
|||
|
||||
deadzone = 0.0
|
||||
|
||||
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
|
||||
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
|
||||
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
|
||||
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
|
||||
pid_log.active = True
|
||||
pid_log.p = self.pid.p
|
||||
pid_log.i = self.pid.i
|
||||
pid_log.f = self.pid.f
|
||||
pid_log.output = output_steer
|
||||
pid_log.saturated = bool(self.pid.saturated)
|
||||
pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
||||
|
||||
return output_steer, angle_steers_des, pid_log
|
||||
|
|
|
|||
|
|
@ -44,8 +44,7 @@ class LongControl():
|
|||
self.long_control_state = LongCtrlState.off # initialized to off
|
||||
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
|
||||
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
|
||||
rate=1/DT_CTRL,
|
||||
sat_limit=0.8)
|
||||
rate=1 / DT_CTRL)
|
||||
self.v_pid = 0.0
|
||||
self.last_output_accel = 0.0
|
||||
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ def apply_deadzone(error, deadzone):
|
|||
return error
|
||||
|
||||
class PIController():
|
||||
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):
|
||||
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100):
|
||||
self._k_p = k_p # proportional gain
|
||||
self._k_i = k_i # integral gain
|
||||
self.k_f = k_f # feedforward gain
|
||||
|
|
@ -25,10 +25,8 @@ class PIController():
|
|||
self.pos_limit = pos_limit
|
||||
self.neg_limit = neg_limit
|
||||
|
||||
self.sat_count_rate = 1.0 / rate
|
||||
self.i_unwind_rate = 0.3 / rate
|
||||
self.i_rate = 1.0 / rate
|
||||
self.sat_limit = sat_limit
|
||||
|
||||
self.reset()
|
||||
|
||||
|
|
@ -40,27 +38,13 @@ class PIController():
|
|||
def k_i(self):
|
||||
return interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
|
||||
def _check_saturation(self, control, check_saturation, error):
|
||||
saturated = (control < self.neg_limit) or (control > self.pos_limit)
|
||||
|
||||
if saturated and check_saturation and abs(error) > 0.1:
|
||||
self.sat_count += self.sat_count_rate
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
|
||||
self.sat_count = clip(self.sat_count, 0.0, 1.0)
|
||||
|
||||
return self.sat_count > self.sat_limit
|
||||
|
||||
def reset(self):
|
||||
self.p = 0.0
|
||||
self.i = 0.0
|
||||
self.f = 0.0
|
||||
self.sat_count = 0.0
|
||||
self.saturated = False
|
||||
self.control = 0
|
||||
|
||||
def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
|
||||
def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
|
||||
self.speed = speed
|
||||
|
||||
error = float(apply_deadzone(setpoint - measurement, deadzone))
|
||||
|
|
@ -81,7 +65,6 @@ class PIController():
|
|||
self.i = i
|
||||
|
||||
control = self.p + self.f + self.i
|
||||
self.saturated = self._check_saturation(control, check_saturation, error)
|
||||
|
||||
self.control = clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
|
|
|
|||
|
|
@ -0,0 +1,44 @@
|
|||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
|
||||
from parameterized import parameterized
|
||||
|
||||
from cereal import car, log
|
||||
from selfdrive.car.car_helpers import interfaces
|
||||
from selfdrive.car.honda.values import CAR as HONDA
|
||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from selfdrive.car.nissan.values import CAR as NISSAN
|
||||
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
|
||||
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
||||
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
||||
|
||||
class TestLatControl(unittest.TestCase):
|
||||
|
||||
@parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
|
||||
def test_saturation(self, car_name, controller):
|
||||
CarInterface, CarController, CarState = interfaces[car_name]
|
||||
CP = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(CP, CarController, CarState)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
controller = controller(CP, CI)
|
||||
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
|
||||
last_actuators = car.CarControl.Actuators.new_message()
|
||||
|
||||
params = log.LiveParametersData.new_message()
|
||||
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, 1, 0)
|
||||
|
||||
self.assertTrue(lac_log.saturated)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1 +1 @@
|
|||
0c0b75ef36ef657208995a5e474ce7dcdc0fc00b
|
||||
2cd40ceff3c326a45c6ddf5f23e7ae2883bf55a2
|
||||
Loading…
Reference in New Issue