diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 050f8747a2d0d1..2e8fe5c9264ab7 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -30,8 +30,12 @@ def __init__(self, CP): self.acc_type = 1 self.lkas_hud = {} + self.cruise_buttons = 0 + self.cruise_buttons_prev = 0 + def update(self, cp, cp_cam): ret = car.CarState.new_message() + self.cruise_buttons_prev = self.cruise_buttons ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) @@ -79,6 +83,8 @@ def update(self, cp, cp_cam): ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"] + self.cruise_buttons = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] + can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b3fd048d996e9..4ab584fbb2f194 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,11 +2,12 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda -from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR +from selfdrive.car.toyota.values import CruiseButtons, Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase EventName = car.CarEvent.EventName +ButtonType = car.CarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -239,6 +240,37 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) ++ buttonEvents = [] ++ ++ #SET / CANCEL ++ if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled: ++ be = car.CarState.ButtonEvent.new_message() ++ be.pressed = False ++ be.type = ButtonType.setCruise ++ buttonEvents.append(be) ++ elif self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: ++ be = car.CarState.ButtonEvent.new_message() ++ be.pressed = True ++ be.type = ButtonType.cancel ++ buttonEvents.append(be) ++ ++ #ACCEL / DECEL ++ if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: ++ be = car.CarState.ButtonEvent.new_message() ++ be.type = ButtonType.unknown ++ if self.CS.cruise_buttons in [CruiseButtons.ACCEL_ACC, CruiseButtons.ACCEL_CC,CruiseButtons.DECEL_ACC, CruiseButtons.DECEL_CC]: ++ be.pressed = True ++ but = self.CS.cruise_buttons ++ else: ++ be.pressed = False ++ but = self.CS.prev_cruise_buttons ++ if but in [CruiseButtons.ACCEL_ACC, CruiseButtons.ACCEL_CC]: ++ be.type = ButtonType.accelCruise ++ elif but in [CruiseButtons.DECEL_ACC, CruiseButtons.DECEL_CC]: ++ be.type = ButtonType.decelCruise ++ buttonEvents.append(be) ++ ++ ret.buttonEvents = buttonEvents # events events = self.create_common_events(ret) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 0639d46ec0c28b..a3c3297f418423 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -35,6 +35,17 @@ class ToyotaFlags(IntFlag): HYBRID = 1 +# Car button codes for COROLLA +class CruiseButtons: + ACCEL_ACC = 9 + DECEL_ACC = 10 + SET_ACC = 8 + ACCEL_CC = 6 + DECEL_CC = 5 + SET_CC = 1 + CANCEL = 0 + + class CAR: # Toyota ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"