From 6ad79d3e81239fb2d8864d42c95f5e1b06e456a2 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Sat, 11 Feb 2023 02:26:50 -0500 Subject: [PATCH] Longitudinal: Reverse ACC Change (#110) --- selfdrive/car/toyota/carcontroller.py | 9 +++++++-- selfdrive/car/toyota/toyotacan.py | 4 ++-- selfdrive/controls/controlsd.py | 6 +++++- selfdrive/controls/lib/drive_helpers.py | 13 ++++++++----- 4 files changed, 22 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2e0d7009c8..c2d262599d 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,5 +1,6 @@ from cereal import car from common.numpy_fast import clip, interp +from common.params import Params from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ @@ -34,6 +35,9 @@ class CarController: self.gas = 0 self.accel = 0 + self.param_s = Params() + self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange") + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -111,15 +115,16 @@ class CarController: # we can spam can to cancel the system even if we are using lat only control if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + reverse_acc = 2 if self._reverse_acc_change else 1 # Lexus IS uses a different cancellation message if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) + can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, reverse_acc)) self.accel = pcm_accel_cmd else: - can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) + can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, reverse_acc)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7e360cc4e1..252beed339 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -27,7 +27,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt): return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, reverse_acc): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, @@ -37,7 +37,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, "CANCEL_REQ": pcm_cancel, - "ALLOW_LONG_PRESS": 1, + "ALLOW_LONG_PRESS": reverse_acc, } return packer.make_can_msg("ACC_CONTROL", 0, values) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d6f8eb9f5b..0877bcba61 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -186,6 +186,8 @@ class Controls: self.experimental_mode = False self.v_cruise_helper = VCruiseHelper(self.CP) + self.reverse_acc_change = False + # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True self.can_log_mono_time = 0 @@ -474,7 +476,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.reverse_acc_change) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -845,6 +847,8 @@ class Controls: self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + self.reverse_acc_change = self.params.get_bool("ReverseAccChange") + # Sample data from sockets and get a carState CS = self.data_sample() cloudlog.timestamp("Data sampled") diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a332d06765..2f40ecf17d 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -51,13 +51,13 @@ class VCruiseHelper: def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric): + def update_v_cruise(self, CS, enabled, is_metric, reverse_acc): self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: if not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric) + self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_acc) self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -67,7 +67,7 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, reverse_acc): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition if not enabled: @@ -77,6 +77,7 @@ class VCruiseHelper: button_type = None v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT + v_cruise_delta_mltplr = 10 if is_metric else 5 for b in CS.buttonEvents: if b.type.raw in self.button_timers and not b.pressed: @@ -103,8 +104,10 @@ class VCruiseHelper: if not self.button_change_states[button_type]["enabled"]: return - v_cruise_delta = v_cruise_delta * (5 if long_press else 1) - if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval + pressed_value = (1 if long_press else v_cruise_delta_mltplr) if reverse_acc else (v_cruise_delta_mltplr if long_press else 1) + long_press_state = not long_press if reverse_acc else long_press + v_cruise_delta = v_cruise_delta * pressed_value + if long_press_state and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta else: self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]