diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ab5bf50f46..6e59ec313e 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -93,7 +93,7 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index feb595359f..81e3cc2f46 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -114,7 +114,7 @@ class CarController: # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled and CS.out.cruiseState.enabled, - hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) + hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw, CS.gac_tr)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 63189bcd89..4295301825 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -92,14 +92,14 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) -def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw): +def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw, gac_tr): target_speed = min(target_speed_kph, 255) values = { "ACCAlwaysOne": 1, "ACCResumeButton": 0, "ACCSpeedSetpoint": target_speed, - "ACCGapLevel": 3 * enabled, # 3 "far", 0 "inactive" + "ACCGapLevel": gac_tr * enabled, # 3 "far", 0 "inactive" "ACCCmdActive": enabled, "ACCAlwaysOne2": 1, "ACCLeadCar": lead_car_in_sight, diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 1f82c63587..1850c275ea 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -251,7 +251,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] @@ -277,6 +277,7 @@ class CarInterface(CarInterfaceBase): if self.CS.prev_lkas_enabled != 1 and self.CS.lkas_enabled == 1: self.CS.madsEnabled = not self.CS.madsEnabled self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + self.toggle_gac(ret, self.CS, bool(self.CS.gap_dist_button), 1, 3, 3, "-") else: self.CS.madsEnabled = False diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index adad1d04fa..2b46071328 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -248,7 +248,7 @@ class CarController: if self.frame % 10 == 0: hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, hud_control.lanesVisible, fcw_display, acc_alert, steer_required, CS.madsEnabled and not CC.latActive) - can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive)) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive, CS.gac_tr)) if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: self.speed = pcm_speed diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 005ad5fb45..8a7c2aa15b 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -102,7 +102,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint): return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) -def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active): +def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active, gac_tr): commands = [] bus_pt = get_pt_bus(CP.carFingerprint) radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl @@ -112,7 +112,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, acc_hud_values = { 'CRUISE_SPEED': hud.v_cruise, 'ENABLE_MINI_CAR': 1 if enabled else 0, - 'HUD_DISTANCE': 0, # max distance setting on display + 'HUD_DISTANCE': gac_tr, # max distance setting on display 'IMPERIAL_UNIT': int(not is_metric), 'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0, 'SET_ME_X01_2': 1, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 629514628a..3e944e4324 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -311,7 +311,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] @@ -333,6 +333,7 @@ class CarInterface(CarInterfaceBase): if self.CS.prev_cruise_setting != 1 and self.CS.cruise_setting == 1: self.CS.madsEnabled = not self.CS.madsEnabled self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + self.toggle_gac(ret, self.CS, (self.CS.cruise_setting == 3), 1, 3, 0, "-") else: self.CS.madsEnabled = False diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 7045cbf105..11a3976757 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -104,7 +104,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s scc11_values = { "MainMode_ACC": 1 if main_enabled else 0, - "TauGapSet": 4, + "TauGapSet": CS.gac_tr, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, "ObjValid": 1, # close lead makes controls tighter diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 19027dc2e7..ad6ed79d9f 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -323,7 +323,7 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] @@ -346,6 +346,7 @@ class CarInterface(CarInterfaceBase): if self.CS.prev_lfa_enabled != 1 and self.CS.lfa_enabled == 1: self.CS.madsEnabled = not self.CS.madsEnabled self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + self.toggle_gac(ret, self.CS, (self.CS.cruise_buttons[-1] == 3), 1, 3, 4, "-") else: self.CS.madsEnabled = False diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 04d8d0453d..b2f207aedc 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,4 +1,5 @@ import yaml +import operator import os import time from abc import abstractmethod, ABC @@ -31,6 +32,8 @@ TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yam TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml') TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.yaml') +GAC_DICT = {1: 1, 2: 2, 3: 3} + def get_torque_params(candidate): with open(TORQUE_SUBSTITUTE_PATH) as f: @@ -100,6 +103,13 @@ class CarInterfaceBase(ABC): self.experimental_mode_hold = False self.experimental_mode = self.param_s.get_bool("ExperimentalMode") self._frame = 0 + self.op_lookup = {"+": operator.add, "-": operator.sub} + self.gac = self.param_s.get_bool("GapAdjustCruise") + self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8"))) + self.prev_gac_button = False + self.gac_button_counter = 0 + self.gac_min = -1 + self.gac_max = -1 @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): @@ -425,12 +435,51 @@ class CarInterfaceBase(ABC): if self.gap_button_counter > 50: self.gap_button_counter = 0 self.experimental_mode_hold = True - self.experimental_mode = self.param_s.get_bool("ExperimentalMode") self.param_s.put_bool("ExperimentalMode", not self.experimental_mode) else: self.gap_button_counter = 0 self.experimental_mode_hold = False + def get_sp_gac_state(self, gac_tr, gac_min, gac_max, inc_dec): + op = self.op_lookup.get(inc_dec) + gac_tr = op(gac_tr, 1) + if inc_dec == "+": + gac_tr = gac_min if gac_tr > gac_max else gac_tr + else: + gac_tr = gac_max if gac_tr < gac_min else gac_tr + return int(gac_tr) + + def get_sp_distance(self, gac_tr, gac_max, gac_dict=None): + if gac_dict is None: + gac_dict = GAC_DICT + for key, value in gac_dict.items(): + if gac_tr == value: + return key + return gac_max + + def toggle_gac(self, cs_out, CS, gac_button, gac_min, gac_max, gac_default, inc_dec): + if (not (self.CP.openpilotLongitudinalControl or self.gac)) or (self.experimental_mode and self.CP.openpilotLongitudinalControl): + cs_out.gapAdjustCruiseTr = 4 + CS.gac_tr = gac_default + return + if self.gac_min != gac_min: + self.gac_min = gac_min + self.param_s.put("GapAdjustCruiseMin", str(self.gac_min)) + if self.gac_max != gac_max: + self.gac_max = gac_max + self.param_s.put("GapAdjustCruiseMax", str(self.gac_max)) + if self.gac_mode in (0, 2): + if gac_button: + self.gac_button_counter += 1 + elif self.prev_gac_button and not gac_button and self.gac_button_counter < 50: + self.gac_button_counter = 0 + CS.gac_tr = self.get_sp_gac_state(CS.gac_tr, gac_min, gac_max, inc_dec) + self.param_s.put("GapAdjustCruiseTr", str(CS.gac_tr)) + else: + self.gac_button_counter = 0 + self.prev_gac_button = gac_button + cs_out.gapAdjustCruiseTr = self.get_sp_distance(CS.gac_tr, gac_max) + def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False, enable_from_brake=False, enable_pressed_long=False, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): @@ -493,11 +542,14 @@ class CarInterfaceBase(ABC): return events, cs_out - def sp_update_params(self): + def sp_update_params(self, CS): + self.experimental_mode = self.param_s.get_bool("ExperimentalMode") + CS.gac_tr = round(float(self.param_s.get("GapAdjustCruiseTr", encoding="utf8"))) self._frame += 1 if self._frame % 300 == 0: self._frame = 0 - self.experimental_mode = self.param_s.get_bool("ExperimentalMode") + self.gac = self.param_s.get_bool("GapAdjustCruise") + self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8"))) class RadarInterfaceBase(ABC): def __init__(self, CP): @@ -529,6 +581,7 @@ class CarStateBase(ABC): self.cluster_speed_hyst_gap = 0.0 self.cluster_min_speed = 0.0 # min speed before dropping to 0 + self.param_s = Params() self.accEnabled = False self.madsEnabled = False self.disengageByBrake = False @@ -536,6 +589,7 @@ class CarStateBase(ABC): self.prev_mads_enabled = False self.control_initialized = False self.gap_dist_button = 0 + self.gac_tr = round(float(self.param_s.get("GapAdjustCruiseTr", encoding="utf8"))) # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) # R = 0.3 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 3d1e24ebac..a96203f17e 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -57,7 +57,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index dbead09803..4ab05e8911 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -44,7 +44,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] #be = car.CarState.ButtonEvent.new_message() diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 267f52c543..7bba999867 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -110,7 +110,7 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d78235325c..f94b31c167 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -121,10 +121,10 @@ class CarController: 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, reverse_acc)) + can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, reverse_acc, CS.gac_send)) self.accel = pcm_accel_cmd else: - can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, reverse_acc)) + can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, reverse_acc, CS.gac_send)) 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/carstate.py b/selfdrive/car/toyota/carstate.py index ddbd8c9299..6680e0742b 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -43,6 +43,10 @@ class CarState(CarStateBase): self.lta_status = False self.prev_lta_status = False self.lta_status_active = False + self.gac_send = False + self.gac_send_counter = 0 + self.follow_distance = 0 + self.follow_distance_converted = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -50,6 +54,7 @@ class CarState(CarStateBase): self.prev_mads_enabled = self.mads_enabled self.prev_lkas_enabled = self.lkas_enabled self.prev_lta_status = self.lta_status + self.prev_gap_dist_button = self.gap_dist_button ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) @@ -155,6 +160,8 @@ class CarState(CarStateBase): if self.CP.flags & ToyotaFlags.SMART_DSU: self.gap_dist_button = cp.vl["SDSU"]["FD_BUTTON"] + self.follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] + # some TSS2 cars have low speed lockout permanently set, so ignore on those cars # these cars are identified by an ACC_TYPE value of 2. # TODO: it is possible to avoid the lockout and gain stop and go if you @@ -336,6 +343,7 @@ class CarState(CarStateBase): signals.append(("SET_SPEED", "PCM_CRUISE_2")) signals.append(("ACC_FAULTED", "PCM_CRUISE_2")) signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2")) + signals.append(("PCM_FOLLOW_DISTANCE", "PCM_CRUISE_2")) checks.append(("PCM_CRUISE_2", 33)) # add gas interceptor reading if we are using it diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a22439a70a..2ec5220062 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -4,13 +4,15 @@ from common.conversions import Conversions as CV from common.params import Params from panda import Panda from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR -from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config, create_mads_event +from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config, create_mads_event from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName GearShifter = car.CarState.GearShifter +GAC_DICT = {3: 1, 2: 2, 1: 3} + class CarInterface(CarInterfaceBase): @staticmethod @@ -244,10 +246,13 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] + if self.CS.gap_dist_button != self.CS.prev_gap_dist_button: + buttonEvents.append(create_button_event(self.CS.gap_dist_button, self.CS.prev_gap_dist_button, {1: ButtonType.gapAdjustCruise})) + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available if ret.cruiseState.available: @@ -263,6 +268,33 @@ class CarInterface(CarInterfaceBase): (self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled): self.CS.madsEnabled = not self.CS.madsEnabled self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + if (not (self.CP.openpilotLongitudinalControl or self.gac)) or (self.experimental_mode and self.CP.openpilotLongitudinalControl): + ret.gapAdjustCruiseTr = 3 + else: + if self.gac_min != 1: + self.gac_min = 1 + self.param_s.put("GapAdjustCruiseMin", str(self.gac_min)) + if self.gac_max != 3: + self.gac_max = 3 + self.param_s.put("GapAdjustCruiseMax", str(self.gac_max)) + if self.gac_mode in (0, 2): + if bool(self.CS.gap_dist_button): + self.gac_button_counter += 1 + elif self.prev_gac_button and not bool(self.CS.gap_dist_button) and self.gac_button_counter < 50: + self.gac_button_counter = 0 + self.CS.follow_distance_converted = self.get_sp_gac_state(self.CS.follow_distance, self.gac_min, self.gac_max, "+") + self.CS.gac_tr = self.get_sp_distance(self.CS.follow_distance_converted, self.gac_max, gac_dict=GAC_DICT) + self.param_s.put("GapAdjustCruiseTr", str(self.CS.gac_tr)) + else: + self.gac_button_counter = 0 + self.prev_gac_button = bool(self.CS.gap_dist_button) + ret.gapAdjustCruiseTr = self.CS.gac_tr + if self.CS.gac_send_counter < 10 and (self.get_sp_distance(ret.gapAdjustCruiseTr, self.gac_max, gac_dict=GAC_DICT) != self.CS.follow_distance): + self.CS.gac_send_counter += 1 + self.CS.gac_send = 1 + else: + self.CS.gac_send_counter = 0 + self.CS.gac_send = 0 else: self.CS.madsEnabled = False diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index ad52dea5ab..44a91d7edf 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -27,12 +27,12 @@ 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, reverse_acc): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, reverse_acc, gac_send): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, "ACC_TYPE": acc_type, - "DISTANCE": 0, + "DISTANCE": gac_send, "MINI_CAR": lead, "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 69e398f464..3557b4d588 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -95,7 +95,7 @@ class CarController: acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - lead_distance)) + lead_distance, CS.gac_tr)) # **** Stock ACC Button Controls **************************************** # diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index f8a83aafb3..80fdc180c6 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -223,7 +223,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) - self.sp_update_params() + self.sp_update_params(self.CS) buttonEvents = [] @@ -247,6 +247,7 @@ class CarInterface(CarInterfaceBase): if not self.CS.prev_mads_enabled and self.CS.mads_enabled: self.CS.madsEnabled = True self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + self.toggle_gac(ret, self.CS, bool(self.CS.gap_dist_button), 1, 3, 3, "-") else: self.CS.madsEnabled = False diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 7d95658b67..fb0f00d53f 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -96,11 +96,11 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, gac_tr): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, - "ACC_Gesetzte_Zeitluecke": 3, + "ACC_Gesetzte_Zeitluecke": gac_tr, "ACC_Display_Prio": 3, "ACC_Abstandsindex": lead_distance, } diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 7c0eaec793..4607635bcb 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -77,7 +77,7 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, gac_tr): values = { "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index bd0a812493..8b7a5c8ab6 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,7 +3,7 @@ import os import numpy as np from common.realtime import sec_since_boot -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from system.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import index_function @@ -64,8 +64,8 @@ def get_stopped_equivalence_factor(v_lead): def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE -def desired_follow_distance(v_ego, v_lead): - return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) +def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW): + return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) def gen_long_model(): @@ -201,6 +201,7 @@ class LongitudinalMpc: def __init__(self, mode='acc'): self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) + self.desired_TF = T_FOLLOW self.reset() self.source = SOURCES[2] @@ -251,11 +252,21 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) + def get_cost_multipliers(self): + TFs = [1.0, 1.25, T_FOLLOW] + # KRKeegan adjustments to costs for different TFs + # these were calculated using the test_longitudinal.py deceleration tests + a_change_tf = interp(self.desired_TF, TFs, [.1, .8, 1.]) + j_ego_tf = interp(self.desired_TF, TFs, [.6, .8, 1.]) + d_zone_tf = interp(self.desired_TF, TFs, [1.6, 1.3, 1.]) + return a_change_tf, j_ego_tf, d_zone_tf + def set_weights(self, prev_accel_constraint=True): if self.mode == 'acc': + cost_mulitpliers = self.get_cost_multipliers() a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost * cost_mulitpliers[0], J_EGO_COST * cost_mulitpliers[1]] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * cost_mulitpliers[2]] elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] @@ -309,13 +320,25 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j): + def update_TF(self, carstate): + gac_tr = carstate.gapAdjustCruiseTr + if gac_tr == 1: + self.desired_TF = 1.0 + elif gac_tr == 2: + self.desired_TF = 1.25 + else: + self.desired_TF = T_FOLLOW + + def update(self, carstate, radarstate, v_cruise, x, v, a, j, prev_accel_constraint): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) + self.update_TF(carstate) + self.set_weights(prev_accel_constraint) + # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. @@ -343,7 +366,7 @@ class LongitudinalMpc: v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.desired_TF) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] @@ -380,6 +403,8 @@ class LongitudinalMpc: self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) self.params[:,4] = T_FOLLOW + if self.mode == 'acc': + self.params[:,4] = self.desired_TF self.run() if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index fc65f4ed5a..07f8c7ff52 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -132,11 +132,10 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] @@ -173,6 +172,7 @@ class LongitudinalPlanner: longitudinalPlan.solverExecutionTime = self.mpc.solve_time longitudinalPlan.e2eX = self.mpc.e2e_x.tolist() + longitudinalPlan.desiredTF = self.mpc.desired_TF longitudinalPlan.visionTurnControllerState = self.vision_turn_controller.state longitudinalPlan.visionTurnSpeed = float(self.vision_turn_controller.v_turn) diff --git a/selfdrive/test/longitudinal_maneuvers/test_accel.py b/selfdrive/test/longitudinal_maneuvers/test_accel.py new file mode 100644 index 0000000000..431cd77684 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/test_accel.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 +import os +import unittest + +from common.params import Params +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + + +maneuvers = [ + # In Stock with jerk cost of 5.0 this results in a maximum desired_dist_diff of 51m + # Setting jerk cost to 0 drops it down to 25m + # Setting jerk cost to .5 drops it down to 38m + Maneuver( + 'Start from standstill behind car acc at 1.2m/s', + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=4, # In real world the stopping distance is less than desired + speed_lead_values=[0., 12., 12.], + breakpoints=[0., 10., 20.], + cruise_values=[35., 35., 35.], + ), +] + + +class LongitudinalControl(unittest.TestCase): + @classmethod + def setUpClass(cls): + os.environ['SIMULATION'] = "1" + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['NO_CAN_TIMEOUT'] = "1" + + params = Params() + params.clear_all() + params.put_bool("Passive", bool(os.getenv("PASSIVE"))) + params.put_bool("OpenpilotEnabledToggle", True) + + # hack + def test_longitudinal_setup(self): + pass + + +def run_maneuver_worker(k): + def run(self): + man = maneuvers[k] + print(man.title) + valid, _ = man.evaluate() + self.assertTrue(valid, msg=man.title) + return run + + +for k in range(len(maneuvers)): + setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}", + run_maneuver_worker(k)) + +if __name__ == "__main__": + unittest.main(failfast=True) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 6621575cde..6bef53d437 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -152,8 +152,11 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { UIScene &scene = s->scene; SubMaster &sm = *(uiState()->sm); auto longitudinal_plan = sm["longitudinalPlan"].getLongitudinalPlan(); + auto car_state = sm["carState"].getCarState(); + auto controls_state = sm["controlsState"].getControlsState(); QRect dlp_btn_rect = QRect(bdr_s * 2 + 220, (rect().bottom() - footer_h / 2 - 75), 150, 150); + QRect gac_btn_rect = QRect(bdr_s * 2 + 220 + 180, (rect().bottom() - footer_h / 2 - 75), 150, 150); QRect debug_tap_rect = QRect(rect().center().x() - 200, rect().center().y() - 200, 400, 400); QRect speed_limit_touch_rect = speed_sgn_rc.adjusted(-50, -50, 50, 50); @@ -162,6 +165,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { scene.dynamic_lane_profile = scene.dynamic_lane_profile > 2 ? 0 : scene.dynamic_lane_profile; params.put("DynamicLaneProfile", std::to_string(scene.dynamic_lane_profile)); propagate_event = false; + } else if (scene.gac && scene.gac_mode != 0 && scene.longitudinal_control && !controls_state.getExperimentalMode() && + car_state.getCruiseState().getAvailable() && gac_btn_rect.contains(e->x(), e->y())) { + scene.gac_tr--; + scene.gac_tr = scene.gac_tr < scene.gac_min ? scene.gac_max : scene.gac_tr; + params.put("GapAdjustCruiseTr", std::to_string(scene.gac_tr)); + propagate_event = false; } else if (longitudinal_plan.getSpeedLimit() > 0.0 && speed_limit_touch_rect.contains(e->x(), e->y())) { // If touching the speed limit sign area when visible scene.last_speed_limit_sign_tap = seconds_since_boot(); @@ -423,6 +432,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("hideVEgoUi", s.scene.hide_vego_ui); + setProperty("gac", s.scene.gac && s.scene.gac_mode != 0 && s.scene.longitudinal_control && !cs.getExperimentalMode() && + car_state.getCruiseState().getAvailable()); + setProperty("gacTr", s.scene.gac_tr); + // update engageability/experimental mode button experimental_btn->updateState(s); @@ -682,6 +695,10 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { drawDlpButton(p, bdr_s * 2 + 220, (rect().bottom() - footer_h / 2 - 75), 150, 150); } + if (gac) { + drawGacButton(p, bdr_s * 2 + 220 + 180, (rect().bottom() - footer_h / 2 - 75), 150, 150); + } + // Stand Still Timer if (standStillTimer && standStill) { drawStandstillTimer(p, rect().right() - 650, 30 + 160 + 250); @@ -790,6 +807,34 @@ void AnnotatedCameraWidget::drawDlpButton(QPainter &p, int x, int y, int w, int p.drawText(dlpBtn, Qt::AlignCenter, dlp_text); } +void AnnotatedCameraWidget::drawGacButton(QPainter &p, int x, int y, int w, int h) { + int prev_gac_tr = -1; + QString gac_text = ""; + QColor gac_border = QColor(255, 255, 255, 255); + + if (prev_gac_tr != gacTr) { + prev_gac_tr = gacTr; + if (gacTr == 1) { + gac_text = "Aggro\nGap"; + gac_border = QColor("#ff4b4b"); + } else if (gacTr == 2) { + gac_text = "Mild\nGap"; + gac_border = QColor("#fcff4b"); + } else { + gac_text = "Stock\nGap"; + gac_border = QColor("#4bff66"); + } + } + + QRect gacBtn(x, y, w, h); + p.setPen(QPen(gac_border, 6)); + p.setBrush(QColor(75, 75, 75, 75)); + p.drawEllipse(gacBtn); + p.setPen(QColor(Qt::white)); + configFont(p, "Inter", 36, "SemiBold"); + p.drawText(gacBtn, Qt::AlignCenter, gac_text); +} + void AnnotatedCameraWidget::drawStandstillTimer(QPainter &p, int x, int y) { char lab_str[16]; char val_str[16]; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 423a7b246d..2996c047ec 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -106,6 +106,9 @@ class AnnotatedCameraWidget : public CameraWidget { Q_PROPERTY(bool hideVEgoUi MEMBER hideVEgoUi); + Q_PROPERTY(bool gac MEMBER gac); + Q_PROPERTY(int gacTr MEMBER gacTr); + public: explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0); void updateState(const UIState &s); @@ -123,6 +126,7 @@ private: bool is_active); void drawDlpButton(QPainter &p, int x, int y, int w, int h); + void drawGacButton(QPainter &p, int x, int y, int w, int h); void drawColoredText(QPainter &p, int x, int y, const QString &text, QColor color); void drawStandstillTimer(QPainter &p, int x, int y); @@ -192,6 +196,9 @@ private: bool hideVEgoUi; + bool gac; + int gacTr; + protected: void paintGL() override; void initializeGL() override; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index e5239e8f65..ec2d4bf530 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -216,6 +216,9 @@ static void update_state(UIState *s) { scene.dynamic_lane_profile = sm["lateralPlan"].getLateralPlan().getDynamicLaneProfile(); scene.dynamic_lane_profile_status = sm["lateralPlan"].getLateralPlan().getDynamicLaneProfileStatus(); } + if (sm.updated("carState")) { + scene.gac_tr = sm["carState"].getCarState().getGapAdjustCruiseTr(); + } } void ui_update_params(UIState *s) { @@ -236,6 +239,10 @@ void ui_update_params(UIState *s) { s->scene.hide_vego_ui = params.getBool("HideVEgoUi"); s->scene.true_vego_ui = params.getBool("TrueVEgoUi"); s->scene.chevron_data = std::atoi(params.get("ChevronInfo").c_str()); + s->scene.gac = params.getBool("GapAdjustCruise"); + s->scene.gac_mode = std::atoi(params.get("GapAdjustCruiseMode").c_str()); + s->scene.gac_min = std::atoi(params.get("GapAdjustCruiseMin").c_str()); + s->scene.gac_max = std::atoi(params.get("GapAdjustCruiseMax").c_str()); if (s->scene.onroadScreenOff > 0) { s->scene.osoTimer = s->scene.onroadScreenOff * 60 * UI_FREQ; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 26135b9103..f57eff9383 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -166,6 +166,9 @@ typedef struct UIScene { bool hide_vego_ui, true_vego_ui; int chevron_data; + + bool gac; + int gac_mode, gac_tr, gac_min, gac_max; } UIScene; class UIState : public QObject {