From 4cb50eeb960972aefa3e30b6def2c5d274de8496 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 15 Sep 2024 03:47:32 -0400 Subject: [PATCH] decouple and implement for mazda --- selfdrive/car/mazda/carcontroller.py | 186 ------------------ selfdrive/car/mazda/interface.py | 2 - selfdrive/car/mazda/mazdacan.py | 10 +- .../__init__.py | 0 .../controller.py | 25 +++ .../helpers.py | 40 ++++ 6 files changed, 69 insertions(+), 194 deletions(-) create mode 100644 sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/__init__.py create mode 100644 sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/controller.py create mode 100644 sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/helpers.py 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/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)