From 6f5c9d5ad152f827ea96f2cc2590468c33b27db5 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 6 Sep 2021 17:29:32 -0700 Subject: [PATCH] Clean up PID controller ACCEL_MIN/ACCEL_MAX (#22148) * use ISO 15622:2018 limits in PID controller * allow more for nidec * limit PID inputs * CP is not needed * add GM * not used * update ref * fix honda bosch old-commit-hash: f941111dcdc9d651fe7169044fb7f52fd70f94ab --- selfdrive/car/gm/interface.py | 6 +++++- selfdrive/car/gm/values.py | 14 ++++++++++++-- selfdrive/car/honda/interface.py | 19 +++++++++++-------- selfdrive/car/honda/values.py | 11 ++++++++++- selfdrive/car/interfaces.py | 4 ++-- selfdrive/car/toyota/interface.py | 6 +++++- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/longcontrol.py | 7 +++++++ selfdrive/test/process_replay/ref_commit | 2 +- 9 files changed, 54 insertions(+), 17 deletions(-) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 0362daf4bc..65030bfdbf 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -2,7 +2,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.gm.values import CAR, CruiseButtons, \ - AccState + AccState, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -10,6 +10,10 @@ ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + params = CarControllerParams() + return params.ACCEL_MIN, params.ACCEL_MAX @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 244dacd16d..15860e6b7e 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -25,10 +25,20 @@ class CarControllerParams(): MAX_GAS = 3072 # Only a safety limit ZERO_GAS = 2048 MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen + + self.ACCEL_MAX = 2.0 # m/s^2 + + # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we + # perform the closed loop control, and might need some + # to apply some more braking if we're on a downhill slope. + # Our controller should still keep the 2 second average above + # -3.5 m/s^2 as per planner limits + self.ACCEL_MIN = -4.0 # m/s^2 + self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle - self.GAS_LOOKUP_BP = [-1.0, 0., 2.0] + self.GAS_LOOKUP_BP = [-1.0, 0., self.ACCEL_MAX] self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] - self.BRAKE_LOOKUP_BP = [-4., -1.0] + self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, -1.0] self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0] class CAR: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index de7bcbe469..fbad2eba61 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -6,7 +6,7 @@ from common.params import Params from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL from selfdrive.car.honda.hondacan import disable_radar from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint -from selfdrive.car.interfaces import CarInterfaceBase, ACCEL_MAX, ACCEL_MIN +from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.config import Conversions as CV @@ -17,12 +17,15 @@ TransmissionType = car.CarParams.TransmissionType class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(current_speed, cruise_speed): - # NIDECs don't allow acceleration near cruise_speed, - # so limit limits of pid to prevent windup - ACCEL_MAX_VALS = [ACCEL_MAX, 0.2] - ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2] - return ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) + def get_pid_accel_limits(CP, current_speed, cruise_speed): + if CP.carFingerprint in HONDA_BOSCH: + return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX + else: + # NIDECs don't allow acceleration near cruise_speed, + # so limit limits of pid to prevent windup + ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2] + ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2] + return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): @@ -54,7 +57,7 @@ class CarInterface(CarInterfaceBase): # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant # unless aTargetMax is very high and then we scale with it; this help in quicker restart - return float(max(max_accel, a_target / CarControllerParams.ACCEL_MAX)) * min(speedLimiter, accelLimiter) + return float(max(max_accel, a_target / CarControllerParams.NIDEC_ACCEL_MAX)) * min(speedLimiter, accelLimiter) @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index da87a7c409..ed0af59c80 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -5,7 +5,16 @@ Ecu = car.CarParams.Ecu VisualAlert = car.CarControl.HUDControl.VisualAlert class CarControllerParams(): - ACCEL_MAX = 1.6 + # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we + # perform the closed loop control, and might need some + # to apply some more braking if we're on a downhill slope. + # Our controller should still keep the 2 second average above + # -3.5 m/s^2 as per planner limits + NIDEC_ACCEL_MIN = -4.0 # m/s^2 + NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons + + BOSCH_ACCEL_MIN = -3.5 # m/s^2 + BOSCH_ACCEL_MAX = 2.0 # m/s^2 def __init__(self, CP): self.BRAKE_MAX = 1024//4 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index b068ee6009..46799aa5ad 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -18,7 +18,7 @@ EventName = car.CarEvent.EventName # model predictions above this speed can be unpredictable MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 135 + 4 = 86 mph ACCEL_MAX = 2.0 -ACCEL_MIN = -4.0 +ACCEL_MIN = -3.5 # generic car and radar interfaces @@ -45,7 +45,7 @@ class CarInterfaceBase(): self.CC = CarController(self.cp.dbc_name, CP, self.VM) @staticmethod - def get_pid_accel_limits(current_speed, cruise_speed): + def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9a22934c17..e0a07cf2d1 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP +from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -9,6 +9,10 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 67aa81f52d..312df7c14b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -460,7 +460,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits) # Steering PID loop and lateral MPC diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 0c76a0b746..4e724afe7e 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -16,6 +16,10 @@ DECEL_STOPPING_TARGET = 2.0 # apply at least this amount of brake to maintain t RATE = 100.0 DEFAULT_LONG_LAG = 0.15 +# As per ISO 15622:2018 for all speeds +ACCEL_MIN_ISO = -3.5 # m/s^2 +ACCEL_MAX_ISO = 2.0 # m/s^2 + # TODO this logic isn't really car independent, does not belong here def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, @@ -82,6 +86,9 @@ class LongControl(): v_target_future = 0.0 a_target = 0.0 + # TODO: This check is not complete and needs to be enforced by MPC + a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO) + self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3023fb4026..32affbb2c0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -858ebd51d52e76edf61190014337df299a7e6374 \ No newline at end of file +bc50c3f64b4677eee32bc49bdf69c3be265fd743 \ No newline at end of file