cleanup old nidec accel override logic (#22181)
* cleanup old nidec accel override logic * add ref without deprecated fields * no more overridepull/22188/head
HaraldSchafer
GitHub
parent
1f639e38b2
commit
74b6c87254
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
f283a33700e03e358b84db0c8fbf3b4d6740ee64
|
||||
f489c4e160cebde60907d3cd2bfbc06bd9d33aad
|
||||
|
|
@ -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]
|
||||
|
|
|
|||
Loading…
Reference in New Issue