diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 423009f502..9e993b89d5 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -212,7 +212,24 @@ struct ModelDataV2SP @0xf98d843bfd7004a3 { modelCapabilities @4 :UInt32; } -struct CustomReserved7 @0xb86e6369214c01c8 { +struct CarControlSP @0xb86e6369214c01c8 { + customStockLongitudinalControl @0 :CustomStockLongitudinalControl; + + struct CustomStockLongitudinalControl { + state @0 :ButtonControlState; + cruiseButton @1 :Int16; + finalSpeedKphDEPRECATED @2 :Float32; + vTarget @3 :Float32; + vCruiseCluster @4 :Float32; + + enum ButtonControlState { + inactive @0; # No button press or default state + loading @1; # Loading state before transitioning to accelerating or decelerating + accelerating @2; # Increasing speed + decelerating @3; # Decreasing speed + holding @4; # Holding steady speed + } + } } struct CustomReserved8 @0xf416ec09499d9d19 { diff --git a/cereal/log.capnp b/cereal/log.capnp index df1aef8796..8e0bf0ac37 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2388,7 +2388,7 @@ struct Event { liveMapDataSP @111 :Custom.LiveMapDataSP; e2eLongStateSP @112 :Custom.E2eLongStateSP; modelV2SP @113 :Custom.ModelDataV2SP; - customReserved7 @114 :Custom.CustomReserved7; + carControlSP @114 :Custom.CarControlSP; customReserved8 @115 :Custom.CustomReserved8; customReserved9 @116 :Custom.CustomReserved9; diff --git a/cereal/services.py b/cereal/services.py index 5bc8dcfac2..386ae57ec5 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -83,6 +83,7 @@ _services: dict[str, tuple] = { "liveMapDataSP": (True, 0.), "e2eLongStateSP": (True, 0.), "modelV2SP": (True, 20., 40), + "carControlSP": (True, 100., 10), # debug "uiDebug": (True, 0., 1), diff --git a/openpilot/sunnypilot b/openpilot/sunnypilot new file mode 120000 index 0000000000..7380b5be6a --- /dev/null +++ b/openpilot/sunnypilot @@ -0,0 +1 @@ +../sunnypilot/ \ No newline at end of file diff --git a/panda b/panda index 55018eafc2..3b4cfcd291 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 55018eafc258f949cbe7bb381fa14ca2805926b9 +Subproject commit 3b4cfcd291095d329100f744716a3e20b64001e0 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 222dcf9619..e844baf2fb 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -31,7 +31,7 @@ class Car: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'carControlSP']) self.can_rcv_cum_timeout_counter = 0 @@ -70,6 +70,12 @@ class Car: if self.CP.customStockLongAvailable and self.CP.pcmCruise and self.params.get_bool("CustomStockLong"): self.CP.pcmCruiseSpeed = False + services = self.sm.data.keys() | {'longitudinalPlanSP'} + self.sm = messaging.SubMaster(list(services)) + + cslc_path = f'openpilot.sunnypilot.selfdrive.car.{self.CP.carName}.custom_stock_longitudinal_controller' + CustomStockLongitudinalController = __import__(cslc_path + '.controller', fromlist=['CustomStockLongitudinalController']).CustomStockLongitudinalController + self.custom_stock_longitudinal_controller = CustomStockLongitudinalController(self, self.CI.CC, self.CI.CS, self.CP) openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") @@ -175,8 +181,16 @@ class Car: # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) + if not self.CP.pcmCruiseSpeed: + can_sends.extend(self.custom_stock_longitudinal_controller.update(CS, CC)) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + cc_send_sp = messaging.new_message('carControlSP') + cc_send_sp.valid = self.sm.all_checks(['carControl']) + if not self.CP.pcmCruiseSpeed: + cc_send_sp.carControlSP.customStockLongitudinalControl = self.custom_stock_longitudinal_controller.state_publish() + self.pm.send('carControlSP', cc_send_sp) + self.CC_prev = CC def step(self): diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 8a805cd2a1..8abc440dfd 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,14 +1,10 @@ from cereal import car -import cereal.messaging as messaging -from common.conversions import Conversions as CV from opendbc.can.packer import CANPacker -from openpilot.common.params import Params -from openpilot.selfdrive.car import DT_CTRL, apply_meas_steer_torque_limits +from openpilot.selfdrive.car import apply_meas_steer_torque_limits from openpilot.selfdrive.car.chrysler import chryslercan from openpilot.selfdrive.car.chrysler.values import RAM_CARS, RAM_DT, CarControllerParams, ChryslerFlags, ChryslerFlagsSP from openpilot.selfdrive.car.interfaces import CarControllerBase, FORWARD_GEARS -from openpilot.selfdrive.controls.lib.drive_helpers import FCA_V_CRUISE_MIN ButtonType = car.CarState.ButtonEvent.Type @@ -26,64 +22,9 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) - self.sm = messaging.SubMaster(['longitudinalPlanSP']) - self.param_s = Params() - self.last_speed_limit_sign_tap_prev = False - self.speed_limit = 0. - self.speed_limit_offset = 0 - self.timer = 0 - self.final_speed_kph = 0 - self.init_speed = 0 - self.current_speed = 0 - self.v_set_dis = 0 - self.v_cruise_min = 0 - self.button_type = 0 - self.button_select = 0 - self.button_count = 0 - self.target_speed = 0 - self.t_interval = 7 - self.slc_active_stock = False - self.sl_force_active_timer = 0 - self.v_tsc_state = 0 - self.slc_state = 0 - self.m_tsc_state = 0 - self.cruise_button = None - self.speed_diff = 0 - self.v_tsc = 0 - self.m_tsc = 0 - self.steady_speed = 0 - self.button_frame = 0 - def update(self, CC, CS, now_nanos): - if not self.CP.pcmCruiseSpeed: - self.sm.update(0) - - if self.sm.updated['longitudinalPlanSP']: - self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed - - self.v_cruise_min = FCA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1) - can_sends = [] - if not self.CP.pcmCruiseSpeed: - if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap: - self.sl_force_active_timer = self.frame - self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False) - self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap - - sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0)) - sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False)) - sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False)) - slc_active = not sl_inactive and not sl_temp_inactive - - self.slc_active_stock = slc_active - lkas_active = CC.latActive and CS.madsEnabled if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS: @@ -114,20 +55,6 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, self.CP, resume=True)) - if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and not self.CP.pcmCruiseSpeed and CS.out.cruiseState.enabled: - self.button_frame += 1 - button_counter_offset = [1, 1, 0, None][self.button_frame % 4] - if ram_cars: - self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) - elif button_counter_offset is not None: - self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) - - if self.cruise_button is not None: - if ram_cars: - can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP, buttons=self.cruise_button)) - elif button_counter_offset is not None: - can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + button_counter_offset, das_bus, self.CP, buttons=self.cruise_button)) - # HUD alerts if self.frame % 25 == 0: if CS.lkas_car_model != -1: @@ -179,122 +106,3 @@ class CarController(CarControllerBase): new_actuators.steerOutputCan = self.apply_steer_last return new_actuators, can_sends - - # multikyd methods, sunnyhaibin logic - def get_cruise_buttons_status(self, CS): - if not CS.out.cruiseState.enabled: - for be in CS.out.buttonEvents: - if be.type in (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) and be.pressed: - self.timer = 40 - elif self.timer: - self.timer -= 1 - else: - return 1 - return 0 - - def get_target_speed(self, v_cruise_kph_prev): - v_cruise_kph = v_cruise_kph_prev - if self.slc_state > 1: - v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH - if not self.slc_active_stock: - v_cruise_kph = v_cruise_kph_prev - return v_cruise_kph - - def get_button_type(self, button_type): - self.type_status = "type_" + str(button_type) - self.button_picker = getattr(self, self.type_status, lambda: "default") - return self.button_picker() - - def reset_button(self): - if self.button_type != 3: - self.button_type = 0 - - def type_default(self): - self.button_type = 0 - return None - - def type_0(self): - self.button_count = 0 - self.target_speed = self.init_speed - self.speed_diff = self.target_speed - self.v_set_dis - if self.target_speed > self.v_set_dis: - self.button_type = 1 - elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min: - self.button_type = 2 - return None - - def type_1(self): - cruise_button = 1 - self.button_count += 1 - if self.target_speed <= self.v_set_dis: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_2(self): - cruise_button = 2 - self.button_count += 1 - if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_3(self): - cruise_button = None - self.button_count += 1 - if self.button_count > self.t_interval: - self.button_type = 0 - return cruise_button - - def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev): - if self.v_tsc_state != 0: - vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH - if int(vision_v_cruise_kph) == int(v_cruise_kph_prev): - vision_v_cruise_kph = 255 - else: - vision_v_cruise_kph = 255 - if self.m_tsc_state > 1: - map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH - if int(map_v_cruise_kph) == 0.0: - map_v_cruise_kph = 255 - else: - map_v_cruise_kph = 255 - curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH) - return min(target_speed_kph, curve_speed) - - def get_button_control(self, CS, final_speed, v_cruise_kph_prev): - self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)) - self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH)) - cruise_button = self.get_button_type(self.button_type) - return cruise_button - - def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)): - if cur_speed > self.steady_speed: - self.steady_speed = cur_speed - elif cur_speed < self.steady_speed - hyst: - self.steady_speed = cur_speed - return self.steady_speed - - def get_cruise_buttons(self, CS, v_cruise_kph_prev): - cruise_button = None - if not self.get_cruise_buttons_status(CS): - pass - elif CS.out.cruiseState.enabled: - set_speed_kph = self.get_target_speed(v_cruise_kph_prev) - if self.slc_state > 1: - target_speed_kph = set_speed_kph - else: - target_speed_kph = min(v_cruise_kph_prev, set_speed_kph) - if self.v_tsc_state != 0 or self.m_tsc_state > 1: - self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev) - else: - self.final_speed_kph = target_speed_kph - - cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses - return cruise_button diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 224b81ad15..e278ffe5d1 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -65,14 +65,9 @@ def create_lkas_command(packer, CP, apply_steer, lkas_control_bit): def create_cruise_buttons(packer, frame, bus, CP, cruise_buttons_msg=None, buttons=0, cancel=False, resume=False): - acc_accel = 1 if buttons == 1 else 0 - acc_decel = 1 if buttons == 2 else 0 - values = { "ACC_Cancel": cancel, "ACC_Resume": resume, - "ACC_Accel": acc_accel, - "ACC_Decel": acc_decel, "COUNTER": frame % 0x10, } diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index cac234a88f..b87cc187e3 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -161,8 +161,6 @@ class CarInterface(CarInterfaceBase): if self.low_speed_alert: events.add(car.CarEvent.EventName.belowSteerSpeed) - ret.customStockLong = self.update_custom_stock_long() - ret.events = events.to_msg() return ret diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 2202b25350..876dff770e 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,16 +1,12 @@ from collections import namedtuple from cereal import car -import cereal.messaging as messaging -from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, rate_limit, make_tester_present_msg, create_gas_interceptor_command from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.drive_helpers import HONDA_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -128,48 +124,7 @@ class CarController(CarControllerBase): self.last_steer = 0.0 self.last_button_frame = 0 - self.sm = messaging.SubMaster(['longitudinalPlanSP']) - self.param_s = Params() - self.last_speed_limit_sign_tap_prev = False - self.speed_limit = 0. - self.speed_limit_offset = 0 - self.timer = 0 - self.final_speed_kph = 0 - self.init_speed = 0 - self.current_speed = 0 - self.v_set_dis = 0 - self.v_cruise_min = 0 - self.button_type = 0 - self.button_select = 0 - self.button_count = 0 - self.target_speed = 0 - self.t_interval = 7 - self.slc_active_stock = False - self.sl_force_active_timer = 0 - self.v_tsc_state = 0 - self.slc_state = 0 - self.m_tsc_state = 0 - self.cruise_button = None - self.speed_diff = 0 - self.v_tsc = 0 - self.m_tsc = 0 - self.steady_speed = 0 - def update(self, CC, CS, now_nanos): - if not self.CP.pcmCruiseSpeed: - self.sm.update(0) - - if self.sm.updated['longitudinalPlanSP']: - self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed - - self.v_cruise_min = HONDA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1) - actuators = CC.actuators hud_control = CC.hudControl conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric) @@ -203,19 +158,6 @@ class CarController(CarControllerBase): apply_steer = int(interp(-limited_steer * self.params.STEER_MAX, self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) - if not self.CP.pcmCruiseSpeed: - if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap: - self.sl_force_active_timer = self.frame - self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False) - self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap - - sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0)) - sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False)) - sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False)) - slc_active = not sl_inactive and not sl_temp_inactive - - self.slc_active_stock = slc_active - # Send CAN commands can_sends = [] @@ -265,15 +207,6 @@ class CarController(CarControllerBase): can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, self.CP.carFingerprint)) elif CC.cruiseControl.resume: can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) - elif CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed: - self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) - if self.cruise_button is not None: - send_freq = 1 - if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2: - send_freq = 3 - if (self.frame - self.last_button_frame) * DT_CTRL > 0.01 * send_freq: - can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, self.cruise_button, self.CP.carFingerprint)) - self.last_button_frame = self.frame else: # Send gas and brake commands. @@ -335,120 +268,3 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends - - # multikyd methods, sunnyhaibin logic - def get_cruise_buttons_status(self, CS): - if not CS.out.cruiseState.enabled or CS.cruise_buttons != 0: - self.timer = 40 - elif self.timer: - self.timer -= 1 - else: - return 1 - return 0 - - def get_target_speed(self, v_cruise_kph_prev): - v_cruise_kph = v_cruise_kph_prev - if self.slc_state > 1: - v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH - if not self.slc_active_stock: - v_cruise_kph = v_cruise_kph_prev - return v_cruise_kph - - def get_button_type(self, button_type): - self.type_status = "type_" + str(button_type) - self.button_picker = getattr(self, self.type_status, lambda: "default") - return self.button_picker() - - def reset_button(self): - if self.button_type != 3: - self.button_type = 0 - - def type_default(self): - self.button_type = 0 - return None - - def type_0(self): - self.button_count = 0 - self.target_speed = self.init_speed - self.speed_diff = self.target_speed - self.v_set_dis - if self.target_speed > self.v_set_dis: - self.button_type = 1 - elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min: - self.button_type = 2 - return None - - def type_1(self): - cruise_button = CruiseButtons.RES_ACCEL - self.button_count += 1 - if self.target_speed <= self.v_set_dis: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_2(self): - cruise_button = CruiseButtons.DECEL_SET - self.button_count += 1 - if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_3(self): - cruise_button = None - self.button_count += 1 - if self.button_count > self.t_interval: - self.button_type = 0 - return cruise_button - - def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev): - if self.v_tsc_state != 0: - vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH - if int(vision_v_cruise_kph) == int(v_cruise_kph_prev): - vision_v_cruise_kph = 255 - else: - vision_v_cruise_kph = 255 - if self.m_tsc_state > 1: - map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH - if int(map_v_cruise_kph) == 0.0: - map_v_cruise_kph = 255 - else: - map_v_cruise_kph = 255 - curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH) - return min(target_speed_kph, curve_speed) - - def get_button_control(self, CS, final_speed, v_cruise_kph_prev): - self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)) - self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH)) - cruise_button = self.get_button_type(self.button_type) - return cruise_button - - def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)): - if cur_speed > self.steady_speed: - self.steady_speed = cur_speed - elif cur_speed < self.steady_speed - hyst: - self.steady_speed = cur_speed - return self.steady_speed - - def get_cruise_buttons(self, CS, v_cruise_kph_prev): - cruise_button = None - if not self.get_cruise_buttons_status(CS): - pass - elif CS.out.cruiseState.enabled: - set_speed_kph = self.get_target_speed(v_cruise_kph_prev) - if self.slc_state > 1: - target_speed_kph = set_speed_kph - else: - target_speed_kph = min(v_cruise_kph_prev, set_speed_kph) - if self.v_tsc_state != 0 or self.m_tsc_state > 1: - self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev) - else: - self.final_speed_kph = target_speed_kph - - cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses - return cruise_button diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 72a1989c9a..7aa59a645b 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -329,8 +329,6 @@ class CarInterface(CarInterfaceBase): if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.add(EventName.manualRestart) - ret.customStockLong = self.update_custom_stock_long() - ret.events = events.to_msg() return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c3cd9a1d94..0dfcaac9d8 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -296,8 +296,6 @@ class CarInterface(CarInterfaceBase): if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache: events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable) - ret.customStockLong = self.update_custom_stock_long() - ret.events = events.to_msg() return ret diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 4bbc9e1c67..4ac07d85f5 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,14 +1,9 @@ from cereal import car -import cereal.messaging as messaging -from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params -from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.mazda import mazdacan from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons -from openpilot.selfdrive.controls.lib.drive_helpers import MAZDA_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert ButtonType = car.CarState.ButtonEvent.Type @@ -21,63 +16,9 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) self.brake_counter = 0 - self.sm = messaging.SubMaster(['longitudinalPlanSP']) - self.param_s = Params() - self.last_speed_limit_sign_tap_prev = False - self.speed_limit = 0. - self.speed_limit_offset = 0 - self.timer = 0 - self.final_speed_kph = 0 - self.init_speed = 0 - self.current_speed = 0 - self.v_set_dis = 0 - self.v_cruise_min = 0 - self.button_type = 0 - self.button_select = 0 - self.button_count = 0 - self.target_speed = 0 - self.t_interval = 7 - self.slc_active_stock = False - self.sl_force_active_timer = 0 - self.v_tsc_state = 0 - self.slc_state = 0 - self.m_tsc_state = 0 - self.cruise_button = None - self.speed_diff = 0 - self.v_tsc = 0 - self.m_tsc = 0 - self.steady_speed = 0 - def update(self, CC, CS, now_nanos): - if not self.CP.pcmCruiseSpeed: - self.sm.update(0) - - if self.sm.updated['longitudinalPlanSP']: - self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed - - self.v_cruise_min = MAZDA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1) - can_sends = [] - if not self.CP.pcmCruiseSpeed: - if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap: - self.sl_force_active_timer = self.frame - self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False) - self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap - - sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0)) - sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False)) - sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False)) - slc_active = not sl_inactive and not sl_temp_inactive - - self.slc_active_stock = slc_active - apply_steer = 0 if CC.latActive: @@ -102,11 +43,6 @@ class CarController(CarControllerBase): # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button when planner wants car to move can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME)) - elif CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed: - self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) - if self.cruise_button is not None: - if self.frame % 10 == 0: - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, self.frame // 10, self.cruise_button)) self.apply_steer_last = apply_steer @@ -128,125 +64,3 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends - - # multikyd methods, sunnyhaibin logic - def get_cruise_buttons_status(self, CS): - if not CS.out.cruiseState.enabled: - for be in CS.out.buttonEvents: - if be.type in (ButtonType.accelCruise, ButtonType.resumeCruise, - ButtonType.decelCruise, ButtonType.setCruise) and be.pressed: - self.timer = 40 - elif self.timer: - self.timer -= 1 - else: - return 1 - return 0 - - def get_target_speed(self, v_cruise_kph_prev): - v_cruise_kph = v_cruise_kph_prev - if self.slc_state > 1: - v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH - if not self.slc_active_stock: - v_cruise_kph = v_cruise_kph_prev - return v_cruise_kph - - def get_button_type(self, button_type): - self.type_status = "type_" + str(button_type) - self.button_picker = getattr(self, self.type_status, lambda: "default") - return self.button_picker() - - def reset_button(self): - if self.button_type != 3: - self.button_type = 0 - - def type_default(self): - self.button_type = 0 - return None - - def type_0(self): - self.button_count = 0 - self.target_speed = self.init_speed - self.speed_diff = self.target_speed - self.v_set_dis - if self.target_speed > self.v_set_dis: - return Buttons.SET_PLUS - self.button_type = 1 - elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min: - return Buttons.SET_MINUS - self.button_type = 2 - return None - - def type_1(self): - cruise_button = Buttons.SET_PLUS - self.button_count += 1 - if self.target_speed <= self.v_set_dis: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_2(self): - cruise_button = Buttons.SET_MINUS - self.button_count += 1 - if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_3(self): - cruise_button = None - self.button_count += 1 - if self.button_count > self.t_interval: - self.button_type = 0 - return cruise_button - - def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev): - if self.v_tsc_state != 0: - vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH - if int(vision_v_cruise_kph) == int(v_cruise_kph_prev): - vision_v_cruise_kph = 255 - else: - vision_v_cruise_kph = 255 - if self.m_tsc_state > 1: - map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH - if int(map_v_cruise_kph) == 0.0: - map_v_cruise_kph = 255 - else: - map_v_cruise_kph = 255 - curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH) - return min(target_speed_kph, curve_speed) - - def get_button_control(self, CS, final_speed, v_cruise_kph_prev): - self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)) - self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH)) - cruise_button = self.get_button_type(self.button_type) - return cruise_button - - def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)): - if cur_speed > self.steady_speed: - self.steady_speed = cur_speed - elif cur_speed < self.steady_speed - hyst: - self.steady_speed = cur_speed - return self.steady_speed - - def get_cruise_buttons(self, CS, v_cruise_kph_prev): - cruise_button = None - if not self.get_cruise_buttons_status(CS): - pass - elif CS.out.cruiseState.enabled: - set_speed_kph = self.get_target_speed(v_cruise_kph_prev) - if self.slc_state > 1: - target_speed_kph = set_speed_kph - else: - target_speed_kph = min(v_cruise_kph_prev, set_speed_kph) - if self.v_tsc_state != 0 or self.m_tsc_state > 1: - self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev) - else: - self.final_speed_kph = target_speed_kph - - cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses - return cruise_button diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 3ce3bd35c9..1bb778448d 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -89,8 +89,6 @@ class CarInterface(CarInterfaceBase): if self.CS.low_speed_alert: events.add(EventName.belowSteerSpeed) - ret.customStockLong = self.update_custom_stock_long() - ret.events = events.to_msg() return ret diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 7c56809918..74f6af04c5 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -92,22 +92,20 @@ def create_button_cmd(packer, CP, counter, button): can = int(button == Buttons.CANCEL) res = int(button == Buttons.RESUME) - inc = int(button == Buttons.SET_PLUS) - dec = int(button == Buttons.SET_MINUS) if CP.flags & MazdaFlags.GEN1: values = { "CAN_OFF": can, "CAN_OFF_INV": (can + 1) % 2, - "SET_P": inc, - "SET_P_INV": (inc + 1) % 2, + "SET_P": 0, + "SET_P_INV": 1, "RES": res, "RES_INV": (res + 1) % 2, - "SET_M": dec, - "SET_M_INV": (dec + 1) % 2, + "SET_M": 0, + "SET_M_INV": 1, "DISTANCE_LESS": 0, "DISTANCE_LESS_INV": 1, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 28c48283a7..8c9b76abb1 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,14 +1,11 @@ from cereal import car -import cereal.messaging as messaging from opendbc.can.packer import CANPacker from openpilot.common.numpy_fast import clip from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags -from openpilot.selfdrive.controls.lib.drive_helpers import VOLKSWAGEN_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -28,70 +25,12 @@ class CarController(CarControllerBase): self.eps_timer_soft_disable_alert = False self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 - self.last_button_frame = 0 - - self.sm = messaging.SubMaster(['longitudinalPlanSP']) - self.param_s = Params() - self.last_speed_limit_sign_tap_prev = False - self.speed_limit = 0. - self.speed_limit_offset = 0 - self.timer = 0 - self.final_speed_kph = 0 - self.init_speed = 0 - self.current_speed = 0 - self.v_set_dis = 0 - self.v_set_dis_prev = 0 - self.v_cruise_min = 0 - self.button_type = 0 - self.button_select = 0 - self.button_count = 0 - self.target_speed = 0 - self.t_interval = 7 - self.slc_active_stock = False - self.sl_force_active_timer = 0 - self.v_tsc_state = 0 - self.slc_state = 0 - self.m_tsc_state = 0 - self.cruise_button = None - self.last_cruise_button = None - self.speed_diff = 0 - self.v_tsc = 0 - self.m_tsc = 0 - self.steady_speed = 0 - self.acc_type = -1 - self.send_count = 0 def update(self, CC, CS, now_nanos): - if not self.CP.pcmCruiseSpeed: - self.sm.update(0) - - if self.sm.updated['longitudinalPlanSP']: - self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed - - self.v_cruise_min = VOLKSWAGEN_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1) actuators = CC.actuators hud_control = CC.hudControl can_sends = [] - if not self.CP.pcmCruiseSpeed: - if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap: - self.sl_force_active_timer = self.frame - self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False) - self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap - - sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0)) - sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False)) - sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False)) - slc_active = not sl_inactive and not sl_temp_inactive - - self.slc_active_stock = slc_active - # **** Steering Controls ************************************************ # if self.frame % self.CCP.STEER_STEP == 0: @@ -171,155 +110,11 @@ class CarController(CarControllerBase): if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume): can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values, cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) - if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled: - if not self.CP.pcmCruiseSpeed: - self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) - if self.cruise_button is not None: - if self.acc_type == -1: - if self.button_count >= 2 and self.v_set_dis_prev != self.v_set_dis: - self.acc_type = 1 if abs(self.v_set_dis - self.v_set_dis_prev) >= 10 and self.last_cruise_button in (1, 2) else \ - 0 if abs(self.v_set_dis - self.v_set_dis_prev) < 10 and self.last_cruise_button not in (1, 2) else 1 - if self.send_count >= 10 and self.v_set_dis_prev == self.v_set_dis: - self.cruise_button = 3 if self.cruise_button == 1 else 4 - if self.acc_type == 0: - self.cruise_button = 1 if self.cruise_button == 1 else 2 # accel, decel - elif self.acc_type == 1: - self.cruise_button = 3 if self.cruise_button == 1 else 4 # resume, set - if self.frame % self.CCP.BTN_STEP == 0: - can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, CS.gra_stock_values, frame=(self.frame // self.CCP.BTN_STEP), - buttons=self.cruise_button, custom_stock_long=True)) - self.send_count += 1 - else: - self.send_count = 0 - self.last_cruise_button = self.cruise_button new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] - self.v_set_dis_prev = self.v_set_dis self.frame += 1 return new_actuators, can_sends - - # multikyd methods, sunnyhaibin logic - def get_cruise_buttons_status(self, CS): - if not CS.out.cruiseState.enabled: - for be in CS.out.buttonEvents: - if be.type in (ButtonType.accelCruise, ButtonType.resumeCruise, - ButtonType.decelCruise, ButtonType.setCruise) and be.pressed: - self.timer = 40 - elif be.type == ButtonType.gapAdjustCruise and be.pressed: - self.timer = 300 - elif self.timer: - self.timer -= 1 - else: - return 1 - return 0 - - def get_target_speed(self, v_cruise_kph_prev): - v_cruise_kph = v_cruise_kph_prev - if self.slc_state > 1: - v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH - if not self.slc_active_stock: - v_cruise_kph = v_cruise_kph_prev - return v_cruise_kph - - def get_button_type(self, button_type): - self.type_status = "type_" + str(button_type) - self.button_picker = getattr(self, self.type_status, lambda: "default") - return self.button_picker() - - def reset_button(self): - if self.button_type != 3: - self.button_type = 0 - - def type_default(self): - self.button_type = 0 - return None - - def type_0(self): - self.button_count = 0 - self.target_speed = self.init_speed - self.speed_diff = self.target_speed - self.v_set_dis - if self.target_speed > self.v_set_dis: - self.button_type = 1 - elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min: - self.button_type = 2 - return None - - def type_1(self): - cruise_button = 1 - self.button_count += 1 - if self.target_speed <= self.v_set_dis: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_2(self): - cruise_button = 2 - self.button_count += 1 - if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min: - self.button_count = 0 - self.button_type = 3 - elif self.button_count > 5: - self.button_count = 0 - self.button_type = 3 - return cruise_button - - def type_3(self): - cruise_button = None - self.button_count += 1 - if self.button_count > self.t_interval: - self.button_type = 0 - return cruise_button - - def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev): - if self.v_tsc_state != 0: - vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH - if int(vision_v_cruise_kph) == int(v_cruise_kph_prev): - vision_v_cruise_kph = 255 - else: - vision_v_cruise_kph = 255 - if self.m_tsc_state > 1: - map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH - if int(map_v_cruise_kph) == 0.0: - map_v_cruise_kph = 255 - else: - map_v_cruise_kph = 255 - curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH) - return min(target_speed_kph, curve_speed) - - def get_button_control(self, CS, final_speed, v_cruise_kph_prev): - self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)) - self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH)) - cruise_button = self.get_button_type(self.button_type) - return cruise_button - - def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)): - if cur_speed > self.steady_speed: - self.steady_speed = cur_speed - elif cur_speed < self.steady_speed - hyst: - self.steady_speed = cur_speed - return self.steady_speed - - def get_cruise_buttons(self, CS, v_cruise_kph_prev): - cruise_button = None - if not self.get_cruise_buttons_status(CS): - pass - elif CS.out.cruiseState.enabled: - set_speed_kph = self.get_target_speed(v_cruise_kph_prev) - if self.slc_state > 1: - target_speed_kph = set_speed_kph - else: - target_speed_kph = min(v_cruise_kph_prev, set_speed_kph) - if self.v_tsc_state != 0 or self.m_tsc_state > 1: - self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev) - else: - self.final_speed_kph = target_speed_kph - - cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses - return cruise_button diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 42b4e96602..058521ca71 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -171,8 +171,6 @@ class CarInterface(CarInterfaceBase): if self.CC.eps_timer_soft_disable_alert: events.add(EventName.steerTimeLimit) - ret.customStockLong = self.update_custom_stock_long() - ret.events = events.to_msg() return ret diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 23ce7f5357..763908b6b2 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -49,7 +49,7 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_p return packer.make_can_msg("LDW_02", bus, values) -def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False): +def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False): values = {s: gra_stock_values[s] for s in [ "GRA_Hauptschalter", # ACC button, on/off "GRA_Typ_Hauptschalter", # ACC main button type @@ -58,18 +58,10 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0 "GRA_ButtonTypeInfo", # unknown related to stalk type ]} - accel_cruise = 1 if buttons == 1 else 0 - decel_cruise = 1 if buttons == 2 else 0 - resume_cruise = 1 if buttons == 3 else 0 - set_cruise = 1 if buttons == 4 else 0 - values.update({ - "COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16, + "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16, "GRA_Abbrechen": cancel, - "GRA_Tip_Wiederaufnahme": resume or resume_cruise, - "GRA_Tip_Setzen": set_cruise, - "GRA_Tip_Runter": decel_cruise, - "GRA_Tip_Hoch": accel_cruise, + "GRA_Tip_Wiederaufnahme": resume, }) return packer.make_can_msg("GRA_ACC_01", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 14c49db7c5..f8d161b970 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -31,7 +31,7 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_p return packer.make_can_msg("LDW_Status", bus, values) -def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False): +def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False): values = {s: gra_stock_values[s] for s in [ "GRA_Hauptschalt", # ACC button, on/off "GRA_Typ_Hauptschalt", # ACC button, momentary vs latching @@ -39,18 +39,10 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0 "GRA_Sender", # ACC button, CAN message originator ]} - accel_cruise = 1 if buttons == 1 else 0 - decel_cruise = 1 if buttons == 2 else 0 - resume_cruise = 1 if buttons == 3 else 0 - set_cruise = 1 if buttons == 4 else 0 - values.update({ - "COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16, + "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16, "GRA_Abbrechen": cancel, - "GRA_Recall": resume or resume_cruise, - "GRA_Neu_Setzen": set_cruise, - "GRA_Down_kurz": decel_cruise, - "GRA_Up_kurz": accel_cruise, + "GRA_Recall": resume, }) return packer.make_can_msg("GRA_Neu", bus, values) diff --git a/sunnypilot/__init__.py b/sunnypilot/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/controls/__init__.py b/sunnypilot/controls/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/controls/lib/__init__.py b/sunnypilot/controls/lib/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py new file mode 100644 index 0000000000..7bccb2a3e8 --- /dev/null +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py @@ -0,0 +1,127 @@ +from abc import abstractmethod, ABC + +from cereal import car, custom +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import ACTIVE_STATES +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.states import InactiveState, \ + LoadingState, AcceleratingState, DeceleratingState, HoldingState +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.helpers import get_set_point, \ + speed_hysteresis, update_manual_button_timers + +ButtonType = car.CarState.ButtonEvent.Type +ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState +SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState +TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState +VisionTurnControllerState = custom.LongitudinalPlanSP.VisionTurnControllerState + +SendCan = tuple[int, bytes, int] + + +class CustomStockLongitudinalControllerBase(ABC): + def __init__(self, car, car_controller, car_state, CP): + self.car = car + self.car_controller = car_controller + self.car_state = car_state + self.CP = CP + + self.v_target = 0 + self.v_cruise_cluster = 0 + self.v_cruise_min = 0 + self.cruise_button = None + self.button_state = ButtonControlState.inactive + + self.v_tsc_state = VisionTurnControllerState.disabled + self.slc_state = SpeedLimitControlState.inactive + self.m_tsc_state = TurnSpeedControlState.inactive + self.v_tsc = 0 + self.speed_limit_offseted = 0 + self.m_tsc = 0 + + self.is_ready = False + self.is_ready_prev = False + self.speed_steady = 0 + + self.accel_button = None + self.decel_button = None + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} + + self.button_states = { + ButtonControlState.inactive: InactiveState(self), + ButtonControlState.loading: LoadingState(self), + ButtonControlState.accelerating: AcceleratingState(self), + ButtonControlState.decelerating: DeceleratingState(self), + ButtonControlState.holding: HoldingState(self), + } + + @abstractmethod + def create_mock_button_messages(self) -> list[SendCan]: + pass + + def state_publish(self) -> custom.CarControlSP.CustomStockLongitudinalControl: + customStockLongitudinalControl = custom.CarControlSP.CustomStockLongitudinalControl.new_message() + customStockLongitudinalControl.state = self.button_state + customStockLongitudinalControl.cruiseButton = 0 if self.cruise_button is None else int(self.cruise_button) + customStockLongitudinalControl.vTarget = float(self.v_target) + customStockLongitudinalControl.vCruiseCluster = float(self.v_cruise_cluster) + return customStockLongitudinalControl + + def update_msgs(self) -> None: + if self.car.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.car.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.car.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.car.sm['longitudinalPlanSP'].turnSpeedControlState + self.v_tsc = self.car.sm['longitudinalPlanSP'].visionTurnSpeed + + speed_limit = self.car.sm['longitudinalPlanSP'].speedLimit + speed_limit_offset = self.car.sm['longitudinalPlanSP'].speedLimitOffset + self.speed_limit_offseted = speed_limit + speed_limit_offset + + self.m_tsc = self.car.sm['longitudinalPlanSP'].turnSpeed + + def update_calculations(self, CS: car.CarState, CC: car.CarControl) -> None: + is_metric = self.car_state.params_list.is_metric + v_cruise_kph = CC.vCruise + v_cruise = v_cruise_kph * CV.KPH_TO_MS + self.v_cruise_min = get_set_point(is_metric) + self.v_cruise_cluster = round(CS.cruiseState.speed * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH)) + + v_targets = {'cruise': CC.vCruise} + + if self.v_tsc_state != VisionTurnControllerState.disabled: + v_targets['v_tsc'] = self.v_tsc + + if self.slc_state in ACTIVE_STATES: + v_targets['slc'] = self.speed_limit_offseted + + if self.m_tsc_state > TurnSpeedControlState.tempInactive: + v_targets['m_tsc'] = self.m_tsc + + source = min(v_targets, key=v_targets.get) + + v_target = speed_hysteresis(self, v_targets[source], self.speed_steady, 1.5 * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)) + v_target = min(v_target, v_cruise) + v_target = round(v_target * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH)) + + self.v_target = v_target + + def update_state(self, CS: car.CarState, CC: car.CarControl) -> None: + update_manual_button_timers(CS, self.cruise_buttons) + + ready = CS.cruiseState.enabled and not CC.cruiseControl.cancel and not CC.cruiseControl.resume + button_pressed = any(self.cruise_buttons[k] > 0 for k in self.cruise_buttons) + self.is_ready = ready and not button_pressed + + self.cruise_button = self.button_states[self.button_state]() + + self.is_ready_prev = self.is_ready + + def update(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]: + self.update_msgs() + + self.update_calculations(CS, CC) + + self.update_state(CS, CC) + + can_sends = self.create_mock_button_messages() + + return can_sends diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py new file mode 100644 index 0000000000..84e4ced9ae --- /dev/null +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py @@ -0,0 +1,30 @@ +from cereal import car + +ButtonType = car.CarState.ButtonEvent.Type + + +def get_set_point(is_metric: bool) -> float: + return 30 if is_metric else 20 + + +def speed_hysteresis(controller, speed: float, speed_steady: float, hyst: float) -> float: + if speed > speed_steady + hyst: + speed_steady = speed - hyst + elif speed < speed_steady - hyst: + speed_steady = speed + hyst + + controller.speed_steady = speed + + return speed_steady + + +def update_manual_button_timers(CS: car.CarState, button_timers: dict[ButtonType, int]) -> None: + # increment timer for buttons still pressed + for k in button_timers: + if button_timers[k] > 0: + button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in button_timers: + # Start/end timer and store current state on change of button pressed + button_timers[b.type.raw] = 1 if b.pressed else 0 diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py new file mode 100644 index 0000000000..a25dd0c315 --- /dev/null +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py @@ -0,0 +1,85 @@ +from abc import ABC, abstractmethod +from random import randint + +from cereal import custom + +ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState + +INACTIVE_TIMER = 40 +RESET_COUNT = 5 +HOLD_TIME = 7 + + +class ButtonStateBase(ABC): + def __init__(self, controller): + self.controller = controller + self.button_count = 0 + self.timer = INACTIVE_TIMER + + def __call__(self) -> int | None: + if self.controller.is_ready: + # TODO-SP: Validate different hold time intervals for different platforms to prevent temporary cruise fault + pass + else: + if self.controller.is_ready_prev: + self.controller.button_state = ButtonControlState.inactive + return None + + return self.handle() + + @abstractmethod + def handle(self) -> int | None: + pass + + +class InactiveState(ButtonStateBase): + def handle(self) -> None: + self.button_count = 0 + + if not self.controller.is_ready: + self.timer = INACTIVE_TIMER + elif self.timer > 0: + self.timer -= 1 + else: + self.controller.button_state = ButtonControlState.loading + return None + + +class LoadingState(ButtonStateBase): + def handle(self) -> None: + if self.controller.v_target > self.controller.v_cruise_cluster: + self.controller.button_state = ButtonControlState.accelerating + elif self.controller.v_target < self.controller.v_cruise_cluster and self.controller.v_cruise_cluster > self.controller.v_cruise_min: + self.controller.button_state = ButtonControlState.decelerating + else: + self.controller.button_state = ButtonControlState.holding + return None + + +class AcceleratingState(ButtonStateBase): + def handle(self) -> int | None: + self.button_count += 1 + if self.controller.v_target <= self.controller.v_cruise_cluster or self.button_count > RESET_COUNT: + self.button_count = 0 + self.controller.button_state = ButtonControlState.holding + return None + return self.controller.accel_button + + +class DeceleratingState(ButtonStateBase): + def handle(self) -> int | None: + self.button_count += 1 + if self.controller.v_target >= self.controller.v_cruise_cluster or self.controller.v_cruise_cluster <= self.controller.v_cruise_min or self.button_count > RESET_COUNT: + self.button_count = 0 + self.controller.button_state = ButtonControlState.holding + return None + return self.controller.decel_button + + +class HoldingState(ButtonStateBase): + def handle(self) -> None: + self.button_count += 1 + if self.button_count > HOLD_TIME: + self.button_count = 0 + self.controller.button_state = ButtonControlState.loading + return None diff --git a/sunnypilot/selfdrive/__init__.py b/sunnypilot/selfdrive/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/__init__.py b/sunnypilot/selfdrive/car/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/chrysler/__init__.py b/sunnypilot/selfdrive/car/chrysler/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..37a34f9a06 --- /dev/null +++ b/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/controller.py @@ -0,0 +1,32 @@ +from cereal import car +from openpilot.selfdrive.car.chrysler.values import RAM_CARS +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \ + SendCan +from openpilot.sunnypilot.selfdrive.car.chrysler.custom_stock_longitudinal_controller import helpers + +ButtonType = car.CarState.ButtonEvent.Type +# TODO-SP: Handle this better in the base class +ACCEL_BUTTON = 1 +DECEL_BUTTON = 2 + + +class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) + self.accel_button = ACCEL_BUTTON + self.decel_button = DECEL_BUTTON + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.resumeCruise: 0} + + def create_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + ram_cars = self.CP.carFingerprint in RAM_CARS + das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0 + button_counter_offset = [1, 1, 0, None][self.car_controller.frame % 4] + + if self.cruise_button is not None: + if ram_cars: + can_sends.append(helpers.create_cruise_buttons(self.car_controller.packer, self.car_state.button_counter, das_bus, self.cruise_button)) + elif button_counter_offset is not None: + can_sends.append(helpers.create_cruise_buttons(self.car_controller.packer, self.car_state.button_counter + button_counter_offset, das_bus, self.cruise_button)) + + return can_sends diff --git a/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/helpers.py b/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/helpers.py new file mode 100644 index 0000000000..779929d3f6 --- /dev/null +++ b/sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/helpers.py @@ -0,0 +1,9 @@ +def create_cruise_buttons(packer, frame, bus, buttons=0): + + values = { + "ACC_Accel": 1 if buttons == 1 else 0, + "ACC_Decel": 1 if buttons == 2 else 0, + "COUNTER": frame % 0x10, + } + + return packer.make_can_msg("CRUISE_BUTTONS", bus, values) diff --git a/sunnypilot/selfdrive/car/ford/__init__.py b/sunnypilot/selfdrive/car/ford/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..6cb6643ee3 --- /dev/null +++ b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/controller.py @@ -0,0 +1,28 @@ +from cereal import car +from openpilot.selfdrive.car.ford.values import CarControllerParams +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \ + SendCan +from openpilot.sunnypilot.selfdrive.car.ford.custom_stock_longitudinal_controller import fordcan + +ButtonType = car.CarState.ButtonEvent.Type +# TODO-SP: Handle this better in the base class +ACCEL_BUTTON = 1 +DECEL_BUTTON = 2 + + +class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) + self.accel_button = ACCEL_BUTTON + self.decel_button = DECEL_BUTTON + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0, + ButtonType.resumeCruise: 0, ButtonType.setCruise: 0} + + def create_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + + if self.cruise_button is not None and (self.car_controller.frame % CarControllerParams.BUTTONS_STEP) == 0: + can_sends.append(fordcan.create_button_msg(self.car_controller.packer, self.car_controller.CAN.camera, self.car_state.buttons_stock_values, buttons=self.cruise_button)) + can_sends.append(fordcan.create_button_msg(self.car_controller.packer, self.car_controller.CAN.main, self.car_state.buttons_stock_values, buttons=self.cruise_button)) + + return can_sends diff --git a/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py new file mode 100644 index 0000000000..9a4815ad2c --- /dev/null +++ b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py @@ -0,0 +1,48 @@ +def create_button_msg(packer, bus: int, stock_values: dict, buttons=0): + """ + Creates a CAN message for the Ford SCCM buttons/switches. + + Includes cruise control buttons, turn lights and more. + + Frequency is 10Hz. + """ + + values = {s: stock_values[s] for s in [ + "HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons + "TurnLghtSwtch_D_Stat", # SCCM Turn signal switch + "WiprFront_D_Stat", + "LghtAmb_D_Sns", + "AccButtnGapDecPress", + "AccButtnGapIncPress", + "AslButtnOnOffCnclPress", + "AslButtnOnOffPress", + "LaSwtchPos_D_Stat", + "CcAslButtnCnclResPress", + "CcAslButtnDeny_B_Actl", + "CcAslButtnIndxDecPress", + "CcAslButtnIndxIncPress", + "CcAslButtnOffCnclPress", + "CcAslButtnOnOffCncl", + "CcAslButtnOnPress", + "CcAslButtnResDecPress", + "CcAslButtnResIncPress", + "CcAslButtnSetDecPress", + "CcAslButtnSetIncPress", + "CcAslButtnSetPress", + "CcButtnOffPress", + "CcButtnOnOffCnclPress", + "CcButtnOnOffPress", + "CcButtnOnPress", + "HeadLghtHiFlash_D_Actl", + "HeadLghtHiOn_B_StatAhb", + "AhbStat_B_Dsply", + "AccButtnGapTogglePress", + "WiprFrontSwtch_D_Stat", + "HeadLghtHiCtrl_D_RqAhb", + ]} + + values.update({ + "CcAslButtnSetIncPress": 1 if buttons == 1 else 0, # CC increase button + "CcAslButtnSetDecPress": 1 if buttons == 2 else 0, # CC decrease button + }) + return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/sunnypilot/selfdrive/car/honda/__init__.py b/sunnypilot/selfdrive/car/honda/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/honda/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/honda/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/honda/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/honda/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..f6437e28bc --- /dev/null +++ b/sunnypilot/selfdrive/car/honda/custom_stock_longitudinal_controller/controller.py @@ -0,0 +1,25 @@ +from cereal import car +from openpilot.selfdrive.car.honda import hondacan +from openpilot.selfdrive.car.honda.values import CruiseButtons +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \ + SendCan +from openpilot.sunnypilot.selfdrive.car.honda.custom_stock_longitudinal_controller import helpers + +ButtonType = car.CarState.ButtonEvent.Type + + +class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) + self.accel_button = CruiseButtons.RES_ACCEL + self.decel_button = CruiseButtons.DECEL_SET + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, + ButtonType.altButton3: 0, ButtonType.cancel: 0} + + def create_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + + if self.cruise_button is not None: + can_sends.append(hondacan.spam_buttons_command(self.car_controller.packer, self.car_controller.CAN, self.cruise_button, self.CP.carFingerprint)) + + return can_sends diff --git a/sunnypilot/selfdrive/car/hyundai/__init__.py b/sunnypilot/selfdrive/car/hyundai/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..70bf22eea8 --- /dev/null +++ b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py @@ -0,0 +1,58 @@ +from random import choices + +from cereal import car +from openpilot.common.numpy_fast import interp +from openpilot.selfdrive.car.hyundai import hyundaicanfd +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CANFD_CAR +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \ + SendCan +from openpilot.sunnypilot.selfdrive.car.hyundai.custom_stock_longitudinal_controller import helpers + +ButtonType = car.CarState.ButtonEvent.Type + +BUTTON_COPIES = 2 +BUTTON_COPIES_TIME = 7 +BUTTON_COPIES_TIME_IMPERIAL = [BUTTON_COPIES_TIME + 3, 70] +BUTTON_COPIES_TIME_METRIC = [BUTTON_COPIES_TIME, 40] +POPULATION = [0, 1] +WEIGHTS = [0.51, 0.49] + + +class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) + self.accel_button = Buttons.RES_ACCEL + self.decel_button = Buttons.SET_DECEL + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0} + + def create_can_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + if self.cruise_button is not None: + copies_xp = BUTTON_COPIES_TIME_METRIC if self.car_state.params_list.is_metric else BUTTON_COPIES_TIME_IMPERIAL + copies = int(interp(BUTTON_COPIES_TIME, copies_xp, [1, BUTTON_COPIES])) + can_sends.extend([helpers.create_clu11(self.car_controller.packer, self.car_state.clu11, self.cruise_button, self.CP)] * copies) + + return can_sends + + def create_canfd_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: + # TODO: resume for alt button cars + pass + else: + if self.cruise_button is not None: + for _ in range(BUTTON_COPIES): + can_sends.append(hyundaicanfd.create_buttons(self.car_controller.packer, self.CP, self.car_controller.CAN, + self.car_state.button_counters + choices(POPULATION, WEIGHTS)[0], + self.cruise_button)) + + return can_sends + + def create_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + if self.CP.carFingerprint in CANFD_CAR: + can_sends.extend(self.create_canfd_mock_button_messages()) + else: + can_sends.extend(self.create_can_mock_button_messages()) + + return can_sends diff --git a/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/helpers.py b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/helpers.py new file mode 100644 index 0000000000..eb4b20b2bd --- /dev/null +++ b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/helpers.py @@ -0,0 +1,22 @@ +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags + +def create_clu11(packer, clu11, button, CP): + values = {s: clu11[s] for s in [ + "CF_Clu_CruiseSwState", + "CF_Clu_CruiseSwMain", + "CF_Clu_SldMainSW", + "CF_Clu_ParityBit1", + "CF_Clu_VanzDecimal", + "CF_Clu_Vanz", + "CF_Clu_SPEED_UNIT", + "CF_Clu_DetentOut", + "CF_Clu_RheostatLevel", + "CF_Clu_CluInfo", + "CF_Clu_AmpInfo", + "CF_Clu_AliveCnt1", + ]} + values["CF_Clu_CruiseSwState"] = button + values["CF_Clu_AliveCnt1"] = (values["CF_Clu_AliveCnt1"] + 1) % 0x10 + # send buttons to camera on camera-scc based cars + bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0 + return packer.make_can_msg("CLU11", bus, values) diff --git a/sunnypilot/selfdrive/car/mazda/__init__.py b/sunnypilot/selfdrive/car/mazda/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..46135fd350 --- /dev/null +++ b/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/controller.py @@ -0,0 +1,25 @@ +from cereal import car +from openpilot.selfdrive.car.mazda.values import Buttons +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \ + SendCan +from openpilot.sunnypilot.selfdrive.car.mazda.custom_stock_longitudinal_controller import helpers + +ButtonType = car.CarState.ButtonEvent.Type + + +class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) + self.accel_button = Buttons.SET_PLUS + self.decel_button = Buttons.SET_MINUS + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, + ButtonType.resumeCruise: 0, ButtonType.setCruise: 0} + + def create_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + + if self.cruise_button is not None: + if self.car_controller.frame % 10 == 0: + can_sends.append(helpers.create_button_cmd(self.car_controller.packer, self.CP, self.car_controller.frame // 10, self.cruise_button)) + + return can_sends diff --git a/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/helpers.py b/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/helpers.py new file mode 100644 index 0000000000..53db341c18 --- /dev/null +++ b/sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/helpers.py @@ -0,0 +1,40 @@ +def create_button_cmd(packer, CP, counter, button): + + can = int(button == Buttons.CANCEL) + res = int(button == Buttons.RESUME) + inc = int(button == Buttons.SET_PLUS) + dec = int(button == Buttons.SET_MINUS) + + if CP.flags & MazdaFlags.GEN1: + values = { + "CAN_OFF": can, + "CAN_OFF_INV": (can + 1) % 2, + + "SET_P": inc, + "SET_P_INV": (inc + 1) % 2, + + "RES": res, + "RES_INV": (res + 1) % 2, + + "SET_M": dec, + "SET_M_INV": (dec + 1) % 2, + + "DISTANCE_LESS": 0, + "DISTANCE_LESS_INV": 1, + + "DISTANCE_MORE": 0, + "DISTANCE_MORE_INV": 1, + + "MODE_X": 0, + "MODE_X_INV": 1, + + "MODE_Y": 0, + "MODE_Y_INV": 1, + + "BIT1": 1, + "BIT2": 1, + "BIT3": 1, + "CTR": (counter + 1) % 16, + } + + return packer.make_can_msg("CRZ_BTNS", 0, values) diff --git a/sunnypilot/selfdrive/car/volkswagen/__init__.py b/sunnypilot/selfdrive/car/volkswagen/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..4b79f6afe0 --- /dev/null +++ b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/controller.py @@ -0,0 +1,77 @@ +from cereal import car, custom +from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \ + SendCan + +ButtonType = car.CarState.ButtonEvent.Type +ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState +# TODO-SP: Handle this better in the base class +ACCEL_BUTTON = 1 +DECEL_BUTTON = 2 +RESUME_CRUISE = 3 +SET_CRUISE = 4 + +ACC_TYPE_BUTTONS = { + 0: {1: ACCEL_BUTTON, 2: DECEL_BUTTON}, # accel, decel + 1: {1: RESUME_CRUISE, 2: SET_CRUISE} # resume, set +} + + +class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): + def __init__(self, car, car_controller, car_state, CP): + super().__init__(car, car_controller, car_state, CP) + self.accel_button = ACCEL_BUTTON + self.decel_button = DECEL_BUTTON + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, + ButtonType.resumeCruise: 0, ButtonType.setCruise: 0} + + self.last_v_cruise_cluster = 0 + self.last_cruise_button = False + + self.acc_type = -1 + self.button_frame = 0 + + self.initial_v_target = 0 + self.initial_v_cruise_cluster = 0 + + def update_checks(self) -> None: + if self.button_state > ButtonControlState.inactive: + self.button_frame += 1 + if self.button_frame == 1: + self.initial_v_target = self.v_target + self.initial_v_cruise_cluster = self.v_cruise_cluster + + self.set_acc_type() + else: + self.button_frame = 0 + self.initial_v_target = 0 + self.initial_v_cruise_cluster = 0 + + def set_acc_type(self): + current_v_cruise_diff = abs(self.v_target - self.v_cruise_cluster) + initial_v_cruise_diff = abs(self.initial_v_target - self.initial_v_cruise_cluster) + + if self.last_v_cruise_cluster != self.v_cruise_cluster and current_v_cruise_diff != initial_v_cruise_diff: + if self.acc_type == -1: + v_cruise_diff = abs(self.v_cruise_cluster - self.last_v_cruise_cluster) + + if v_cruise_diff >= 10 and self.last_cruise_button in (ACCEL_BUTTON, DECEL_BUTTON): + self.acc_type = 1 + else: + self.acc_type = 0 + + if self.cruise_button is not None: + self.cruise_button = ACC_TYPE_BUTTONS.get(self.acc_type, {}).get(self.cruise_button, self.cruise_button) + + def create_mock_button_messages(self) -> list[SendCan]: + can_sends = [] + + self.update_checks() + + if self.cruise_button is not None: + if self.car_state.gra_stock_values["COUNTER"] != self.car_controller.gra_acc_counter_last: + can_sends.append(self.car_controller.CCS.create_acc_buttons_control(self.car_controller.packer_pt, self.car_controller.ext_bus, self.car_state.gra_stock_values, buttons=self.cruise_button)) + + self.last_cruise_button = self.cruise_button + self.last_v_cruise_cluster = self.v_cruise_cluster + + return can_sends diff --git a/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/mqbcan.py b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/mqbcan.py new file mode 100644 index 0000000000..976057b068 --- /dev/null +++ b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/mqbcan.py @@ -0,0 +1,18 @@ +def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0): + values = {s: gra_stock_values[s] for s in [ + "GRA_Hauptschalter", # ACC button, on/off + "GRA_Typ_Hauptschalter", # ACC main button type + "GRA_Codierung", # ACC button configuration/coding + "GRA_Tip_Stufe_2", # unknown related to stalk type + "GRA_ButtonTypeInfo", # unknown related to stalk type + ]} + + values.update({ + "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16, + "GRA_Tip_Wiederaufnahme": 1 if buttons == 3 else 0, + "GRA_Tip_Setzen": 1 if buttons == 4 else 0, + "GRA_Tip_Runter": 1 if buttons == 2 else 0, + "GRA_Tip_Hoch": 1 if buttons == 1 else 0, + }) + + return packer.make_can_msg("GRA_ACC_01", bus, values) diff --git a/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/pqcan.py b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/pqcan.py new file mode 100644 index 0000000000..7f3f20156c --- /dev/null +++ b/sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/pqcan.py @@ -0,0 +1,17 @@ +def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0): + values = {s: gra_stock_values[s] for s in [ + "GRA_Hauptschalt", # ACC button, on/off + "GRA_Typ_Hauptschalt", # ACC button, momentary vs latching + "GRA_Kodierinfo", # ACC button, configuration + "GRA_Sender", # ACC button, CAN message originator + ]} + + values.update({ + "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16, + "GRA_Recall": 1 if buttons == 3 else 0, + "GRA_Neu_Setzen": 1 if buttons == 4 else 0, + "GRA_Down_kurz": 1 if buttons == 2 else 0, + "GRA_Up_kurz": 1 if buttons == 1 else 0, + }) + + return packer.make_can_msg("GRA_Neu", bus, values)