diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index b0b3d4a..5728fff 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -440,6 +440,8 @@ class CarStateBase(ABC): self.v_ego_clu_kf = KF1D(x0=x0, A=A, C=C[0], K=K) self.softHoldActive = 0 + self.is_metric = True + self.lkas_enabled = False @abstractmethod def update(self, can_parsers) -> structs.CarState: diff --git a/opendbc_repo/opendbc/car/mazda/carcontroller.py b/opendbc_repo/opendbc/car/mazda/carcontroller.py index f2ea870..c003297 100644 --- a/opendbc_repo/opendbc/car/mazda/carcontroller.py +++ b/opendbc_repo/opendbc/car/mazda/carcontroller.py @@ -3,6 +3,8 @@ from opendbc.car import Bus, apply_driver_steer_torque_limits, structs from opendbc.car.interfaces import CarControllerBase from opendbc.car.mazda import mazdacan from opendbc.car.mazda.values import CarControllerParams, Buttons +from opendbc.car.common.conversions import Conversions as CV +from openpilot.common.params import Params VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -13,8 +15,16 @@ class CarController(CarControllerBase): self.apply_steer_last = 0 self.packer = CANPacker(dbc_names[Bus.pt]) self.brake_counter = 0 + + self.activateCruise = 0 + self.speed_from_pcm = 1 def update(self, CC, CS, now_nanos): + + if self.frame % 50 == 0: + params = Params() + self.speed_from_pcm = params.get_int("SpeedFromPCM") + can_sends = [] apply_steer = 0 @@ -35,12 +45,18 @@ class CarController(CarControllerBase): # Cancel Stock ACC if it's enabled while OP is disengaged # Send at a rate of 10hz until we sync with stock ACC state can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL)) - else: + elif False: self.brake_counter = 0 if CC.cruiseControl.resume and self.frame % 5 == 0: # 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)) + else: + if self.frame % 20 == 0: + spam_button = self.make_spam_button(CC, CS) + if spam_button > 0: + self.brake_counter = 0 + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, self.frame // 10, spam_button)) self.apply_steer_last = apply_steer @@ -62,3 +78,36 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends + + def make_spam_button(self, CC, CS): + hud_control = CC.hudControl + set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + target = int(set_speed_in_units+0.5) + target = int(round(target / 5.0) * 5.0) + current = int(CS.out.cruiseState.speed*CV.MS_TO_KPH + 0.5) + current = int(round(current / 5.0) * 5.0) + v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH + + cant_activate = CS.out.brakePressed or CS.out.gasPressed + + if CC.enabled: + if not CS.out.cruiseState.enabled: + if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0 and not cant_activate: + self.activateCruise = 1 + print("RESUME") + return Buttons.RESUME + elif CC.cruiseControl.resume: + return Buttons.RESUME + elif target < current and current>= 31 and self.speed_from_pcm != 1: + print(f"SET_MINUS target={target}, current={current}") + return Buttons.SET_MINUS + elif target > current and current < 160 and self.speed_from_pcm != 1: + print(f"SET_PLUS target={target}, current={current}") + return Buttons.SET_PLUS + elif CS.out.activateCruise: + if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0 and not cant_activate: + self.activateCruise = 1 + print("RESUME") + return Buttons.RESUME + + return 0 diff --git a/opendbc_repo/opendbc/car/mazda/carstate.py b/opendbc_repo/opendbc/car/mazda/carstate.py index c123ee6..8ac7907 100644 --- a/opendbc_repo/opendbc/car/mazda/carstate.py +++ b/opendbc_repo/opendbc/car/mazda/carstate.py @@ -3,10 +3,11 @@ from opendbc.can.parser import CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase -from opendbc.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags +from opendbc.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, Buttons ButtonType = structs.CarState.ButtonEvent.Type - +BUTTONS_DICT = {Buttons.SET_PLUS: ButtonType.accelCruise, Buttons.SET_MINUS: ButtonType.decelCruise, + Buttons.RESUME: ButtonType.resumeCruise, Buttons.CANCEL: ButtonType.cancel} class CarState(CarStateBase): def __init__(self, CP): @@ -19,7 +20,9 @@ class CarState(CarStateBase): self.acc_active_last = False self.low_speed_alert = False self.lkas_allowed_speed = False - + self.lkas_disabled = False + + self.prev_distance_button = 0 self.distance_button = 0 def update(self, can_parsers) -> structs.CarState: @@ -28,9 +31,20 @@ class CarState(CarStateBase): ret = structs.CarState() - prev_distance_button = self.distance_button + self.prev_distance_button = self.distance_button self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] + + self.prev_cruise_buttons = self.cruise_buttons + if bool(cp.vl["CRZ_BTNS"]["SET_P"]): + self.cruise_buttons = Buttons.SET_PLUS + elif bool(cp.vl["CRZ_BTNS"]["SET_M"]): + self.cruise_buttons = Buttons.SET_MINUS + elif bool(cp.vl["CRZ_BTNS"]["RES"]): + self.cruise_buttons = Buttons.RESUME + else: + self.cruise_buttons = Buttons.NONE + ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], cp.vl["WHEEL_SPEEDS"]["FR"], @@ -113,13 +127,21 @@ class CarState(CarStateBase): self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] # camera signals + self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 self.cam_lkas = cp_cam.vl["CAM_LKAS"] self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = not self.lkas_disabled + # TODO: add button types for inc and dec - ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) - + #ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.cruise_buttons, self.prev_cruise_buttons, BUTTONS_DICT), + *create_button_events(self.distance_button, self.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.lkas_enabled, self.lkas_previously_enabled, {1: ButtonType.lfaButton}), + ] return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/mazda/mazdacan.py b/opendbc_repo/opendbc/car/mazda/mazdacan.py index 0605d98..f3be525 100644 --- a/opendbc_repo/opendbc/car/mazda/mazdacan.py +++ b/opendbc_repo/opendbc/car/mazda/mazdacan.py @@ -92,20 +92,22 @@ 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": 0, - "SET_P_INV": 1, + "SET_P": inc, + "SET_P_INV": (inc + 1) % 2, "RES": res, "RES_INV": (res + 1) % 2, - "SET_M": 0, - "SET_M_INV": 1, + "SET_M": dec, + "SET_M_INV": (dec + 1) % 2, "DISTANCE_LESS": 0, "DISTANCE_LESS_INV": 1, diff --git a/panda/board/safety.h b/panda/board/safety.h index 69cda6c..9886d93 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -780,7 +780,7 @@ void pcm_cruise_check(bool cruise_engaged) { // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages if (!cruise_engaged) { controls_allowed = false; - print("controls_allowed(pcm) = false\n"); + //print("controls_allowed(pcm) = false\n"); } if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = true; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index ed87451..3d26f19 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -83,7 +83,7 @@ static bool mazda_tx_hook(const CANPacket_t *to_send) { // only allow cancel while contrls not allowed bool cancel_cmd = (GET_BYTE(to_send, 0) == 0x1U); if (!controls_allowed && !cancel_cmd) { - tx = false; + //tx = false; } } }