From cb1cd01bdf9f030fdb2eabc095e599f3a74e16e6 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 10 Sep 2021 10:34:18 -0700 Subject: [PATCH] cleanup old nidec accel override logic (#22181) * cleanup old nidec accel override logic * add ref without deprecated fields * no more override old-commit-hash: 74b6c872545ac64055a914789a89ccc173168d8b --- selfdrive/car/honda/carcontroller.py | 5 ++-- selfdrive/car/honda/carstate.py | 13 --------- selfdrive/car/honda/interface.py | 35 ------------------------ selfdrive/car/interfaces.py | 4 --- selfdrive/controls/controlsd.py | 14 +--------- selfdrive/controls/lib/longcontrol.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- tools/replay/ui.py | 7 ++--- 8 files changed, 8 insertions(+), 74 deletions(-) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b57bd79aca..eb94451a1c 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -107,8 +107,7 @@ class CarController(): self.params = CarControllerParams(CP) - def update(self, enabled, CS, frame, actuators, - pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, + def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params @@ -228,6 +227,8 @@ class CarController(): apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) + + pcm_override = True can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index e9766e0731..4c8cc1ebac 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -10,18 +10,6 @@ from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, TransmissionType = car.CarParams.TransmissionType -def calc_cruise_offset(offset, speed): - # heuristic formula so that speed is controlled to ~ 0.3m/s below pid_speed - # constraints to solve for _K0, _K1, _K2 are: - # - speed = 0m/s, out = -0.3 - # - speed = 34m/s, offset = 20, out = -0.25 - # - speed = 34m/s, offset = -2.5, out = -1.8 - _K0 = -0.3 - _K1 = -0.01879 - _K2 = 0.01013 - return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.) - - def get_can_signals(CP, gearbox_msg="GEARBOX"): # this function generates lists for signal, messages and initial values signals = [ @@ -317,7 +305,6 @@ class CarState(CarStateBase): ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"]["CRUISE_SPEED"] * CV.KPH_TO_MS self.v_cruise_pcm_prev = ret.cruiseState.speed else: - ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]["CRUISE_SPEED_OFFSET"], ret.vEgo) ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS self.brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index f99e6f75bb..d372278e65 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -27,38 +27,6 @@ class CarInterface(CarInterfaceBase): 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): - - # normalized max accel. Allowing max accel at low speed causes speed overshoots - max_accel_bp = [10, 20] # m/s - max_accel_v = [0.714, 1.0] # unit of max accel - max_accel = interp(v_ego, max_accel_bp, max_accel_v) - - # limit the pcm accel cmd if: - # - v_ego exceeds v_target, or - # - a_ego exceeds a_target and v_ego is close to v_target - - eA = a_ego - a_target - valuesA = [1.0, 0.1] - bpA = [0.3, 1.1] - - eV = v_ego - v_target - valuesV = [1.0, 0.1] - bpV = [0.0, 0.5] - - valuesRangeV = [1., 0.] - bpRangeV = [-1., 0.] - - # only limit if v_ego is close to v_target - speedLimiter = interp(eV, bpV, valuesV) - accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) - - # 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.NIDEC_ACCEL_MAX)) * min(speedLimiter, accelLimiter) - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) @@ -439,10 +407,7 @@ class CarInterface(CarInterfaceBase): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.speedOverride, - c.cruiseControl.override, c.cruiseControl.cancel, - c.cruiseControl.accelOverride, hud_v_cruise, c.hudControl.lanesVisible, hud_show_car=c.hudControl.leadVisible, diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f74fe06e03..ee9fbeb1d8 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -48,10 +48,6 @@ class CarInterfaceBase(): def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX - @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1. - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): raise NotImplementedError diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e2737ee053..45218d6150 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -149,8 +149,6 @@ class Controls: self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = False - self.v_target = 0.0 - self.a_target = 0.0 # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True @@ -464,7 +462,7 @@ class Controls: if not self.joystick_mode: # accel PID loop 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) + actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits) # Steering PID loop and lateral MPC desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, @@ -527,20 +525,10 @@ class Controls: CC.enabled = self.enabled CC.actuators = actuators - CC.cruiseControl.override = True CC.cruiseControl.cancel = not self.CP.pcmCruise or (not self.enabled and CS.cruiseState.enabled) if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: CC.cruiseControl.cancel = True - # TODO remove car specific stuff in controls - # Some override values for Honda - # brake discount removes a sharp nonlinearity - brake_discount = (1.0 - clip(-actuators.accel * (3.0/4.0), 0.0, 1.0)) - speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) - CC.cruiseControl.speedOverride = float(speed_override if self.CP.pcmCruise else 0.0) - CC.cruiseControl.accelOverride = float(self.CI.calc_accel_override(CS.aEgo, self.a_target, - CS.vEgo, self.v_target)) - CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = self.enabled CC.hudControl.lanesVisible = self.enabled diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 28cd42b107..97ed694468 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -136,4 +136,4 @@ class LongControl(): self.last_output_accel = output_accel final_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - return final_accel, v_target, a_target + return final_accel diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a3169063e1..6c41486c62 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f283a33700e03e358b84db0c8fbf3b4d6740ee64 \ No newline at end of file +f489c4e160cebde60907d3cd2bfbc06bd9d33aad \ No newline at end of file diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 2809390292..f1dc0dbf49 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -80,14 +80,13 @@ def ui_thread(addr, frame_address): "v_override": 10, "v_cruise": 11, "a_ego": 12, - "a_target": 13, - "accel_override": 14} + "a_target": 13} plot_arr = np.zeros((100, len(name_to_arr_idx.values()))) plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])] plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)] - plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"], + plot_names = [["gas", "computer_gas", "user_brake", "computer_brake"], ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"], ["v_ego", "v_override", "v_pid", "v_cruise"], ["a_ego", "a_target"]] @@ -155,10 +154,8 @@ def ui_thread(addr, frame_address): plot_arr[-1, name_to_arr_idx['computer_brake']] = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0) plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid - plot_arr[-1, name_to_arr_idx['v_override']] = sm['carControl'].cruiseControl.speedOverride plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo - plot_arr[-1, name_to_arr_idx['accel_override']] = sm['carControl'].cruiseControl.accelOverride if len(sm['longitudinalPlan'].accels): plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]