diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 9832cebe46..368198e1c8 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -11,8 +11,7 @@ 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 - -BUTTONS_STATES = ["accelCruise", "decelCruise", "cancel", "resumeCruise"] +ButtonType = car.CarState.ButtonEvent.Type class CarController(CarControllerBase): @@ -141,8 +140,10 @@ class CarController(CarControllerBase): # multikyd methods, sunnyhaibin logic def get_cruise_buttons_status(self, CS): if not CS.out.cruiseState.enabled: - if any(CS.buttonStates[button_state] for button_state in BUTTONS_STATES): - self.timer = 40 + 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: diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index dea768c03a..6fd287fa7c 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, BUTTON_STATES +from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, BUTTONS class CarState(CarStateBase): def __init__(self, CP): @@ -24,8 +24,7 @@ class CarState(CarStateBase): self.lkas_enabled = False self.prev_lkas_enabled = False - self.buttonStates = BUTTON_STATES.copy() - self.buttonStatesPrev = BUTTON_STATES.copy() + self.button_states = {button.event_type: False for button in BUTTONS} def update(self, cp, cp_cam): @@ -36,7 +35,6 @@ class CarState(CarStateBase): self.prev_mads_enabled = self.mads_enabled self.prev_lkas_enabled = self.lkas_enabled - self.buttonStatesPrev = self.buttonStates.copy() ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], @@ -56,6 +54,18 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEAR"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + # Buttons + button_events = [] + for button in BUTTONS: + state = (cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + button_events.append(event) + self.button_states[button.event_type] = state + self.button_events = button_events + ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"]) ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS_STATUS"] != 0 ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS_STATUS"] != 0 @@ -114,11 +124,6 @@ class CarState(CarStateBase): self.acc_active_last = ret.cruiseState.enabled - self.buttonStates["accelCruise"] = bool(cp.vl["CRZ_BTNS"]["SET_P"]) - self.buttonStates["decelCruise"] = bool(cp.vl["CRZ_BTNS"]["SET_M"]) - self.buttonStates["cancel"] = bool(cp.vl["CRZ_BTNS"]["CAN_OFF"]) - self.buttonStates["resumeCruise"] = bool(cp.vl["CRZ_BTNS"]["RES"]) - self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] # camera signals diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index bb9a3f93a0..f8919d13e7 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS, BUTTON_STATES +from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -12,7 +12,6 @@ GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) - self.buttonStatesPrev = BUTTON_STATES.copy() @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): @@ -41,19 +40,15 @@ class CarInterface(CarInterfaceBase): self.sp_update_params() # TODO: add button types for inc and dec - buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) - - for button in self.CS.buttonStates: - if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: - be = car.CarState.ButtonEvent.new_message() - be.type = button - be.pressed = self.CS.buttonStates[button] - buttonEvents.append(be) + self.CS.button_events = [ + *self.CS.button_events, + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ] self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise) + self.CS.button_events, c.vCruise) if ret.cruiseState.available: if self.enable_mads: @@ -66,7 +61,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = False if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -82,14 +77,14 @@ class CarInterface(CarInterfaceBase): # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) + self.CS.button_events.append(create_mads_event(self.mads_event_lock)) self.mads_event_lock = False else: if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) + self.CS.button_events.append(create_mads_event(self.mads_event_lock)) self.mads_event_lock = True - ret.buttonEvents = buttonEvents + ret.buttonEvents = self.CS.button_events # events events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], @@ -108,6 +103,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - self.buttonStatesPrev = self.CS.buttonStates.copy() - return ret diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 2aac951dbb..2889cbb7b2 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,3 +1,4 @@ +from collections import namedtuple from dataclasses import dataclass, field from enum import IntFlag @@ -8,6 +9,7 @@ from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarPar from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) # Steer torque limits @@ -26,14 +28,6 @@ class CarControllerParams: pass -BUTTON_STATES = { - "accelCruise": False, - "decelCruise": False, - "cancel": False, - "resumeCruise": False, -} - - @dataclass class MazdaCarDocs(CarDocs): package: str = "All" @@ -98,6 +92,14 @@ class Buttons: CANCEL = 4 +BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.accelCruise, "CRZ_BTNS", "SET_P", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "CRZ_BTNS", "SET_M", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "CRZ_BTNS", "CAN_OFF", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "CRZ_BTNS", "RES", [1]), +] + + FW_QUERY_CONFIG = FwQueryConfig( requests=[ # TODO: check data to ensure ABS does not skip ISO-TP frames on bus 0