From 9de8f8cd8c384596868edf3307da51819b28bb4b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 26 Jan 2022 08:10:41 -0800 Subject: [PATCH] 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 --- release/files_common | 1 + selfdrive/car/ford/interface.py | 2 +- selfdrive/car/nissan/interface.py | 2 +- selfdrive/car/tesla/interface.py | 1 + selfdrive/controls/controlsd.py | 22 ++-------- selfdrive/controls/lib/latcontrol.py | 28 ++++++++++++ selfdrive/controls/lib/latcontrol_angle.py | 17 +++---- selfdrive/controls/lib/latcontrol_indi.py | 34 +++++--------- selfdrive/controls/lib/latcontrol_lqr.py | 30 +++---------- selfdrive/controls/lib/latcontrol_pid.py | 15 ++++--- selfdrive/controls/lib/longcontrol.py | 3 +- selfdrive/controls/lib/pid.py | 21 +-------- .../controls/lib/tests/test_latcontrol.py | 44 +++++++++++++++++++ selfdrive/test/process_replay/ref_commit | 2 +- 14 files changed, 116 insertions(+), 106 deletions(-) create mode 100644 selfdrive/controls/lib/latcontrol.py create mode 100755 selfdrive/controls/lib/tests/test_latcontrol.py diff --git a/release/files_common b/release/files_common index c789fc010..8c02ec332 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 12cf6367e..0faaa3f66 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.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 diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 4350fb544..18e19103e 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -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 diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index e8d6ab685..2a19cb35f 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index eda399140..0e9b0755e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py new file mode 100644 index 000000000..983437890 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index dc184cf58..c93535631 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 75b27ac8c..c1f5384cc 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 5777cde8e..5c273a45b 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index bcb8ccba5..f5ff5a95e 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -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 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index b14ffdc6c..21c34aa2d 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index e384aaaed..28c643819 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -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 diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py new file mode 100755 index 000000000..8345840ec --- /dev/null +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -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() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d2df07ae3..a5dca1cc4 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0c0b75ef36ef657208995a5e474ce7dcdc0fc00b \ No newline at end of file +2cd40ceff3c326a45c6ddf5f23e7ae2883bf55a2 \ No newline at end of file