diff --git a/common/params.cc b/common/params.cc index 35ceecfdc3..f8c66313ed 100644 --- a/common/params.cc +++ b/common/params.cc @@ -199,6 +199,11 @@ std::unordered_map keys = { {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, + + {"AccMadsCombo", PERSISTENT}, + {"BelowSpeedPause", PERSISTENT}, + {"DisengageLateralOnBrake", PERSISTENT}, + {"EnableMads", PERSISTENT}, }; } // namespace diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index bc454aa54e..b6a5b99072 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -366,6 +366,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setIgnitionLine(health.ignition_line_pkt); ps.setIgnitionCan(health.ignition_can_pkt); ps.setControlsAllowed(health.controls_allowed_pkt); + ps.setControlsAllowedLong(health.controls_allowed_long_pkt); ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt); ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index ba6aaf8250..2559977206 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -41,7 +41,7 @@ class CarController: # HUD alerts if self.frame % 25 == 0: if CS.lkas_car_model != -1: - can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) + can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CS.madsEnabled, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) self.hud_count += 1 # steering diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 0f0d30782a..54a381cd00 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -25,6 +25,8 @@ class CarState(CarStateBase): ret = car.CarState.new_message() + self.prev_mads_enabled = self.mads_enabled + # lock info ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], cp.vl["BCM_1"]["DOOR_OPEN_FR"], @@ -58,8 +60,8 @@ class CarState(CarStateBase): ) # button presses - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1, - cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2) + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1, + cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2) ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1 # steering wheel diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 10ed73e9f2..a1e9e90daa 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -4,7 +4,7 @@ from selfdrive.car.chrysler.values import RAM_CARS GearShifter = car.CarState.GearShifter VisualAlert = car.CarControl.HUDControl.VisualAlert -def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam): +def create_lkas_hud(packer, CP, lkas_active, mads_enabled, hud_alert, hud_count, car_model, auto_high_beam): # LKAS_HUD - Controls what lane-keeping icon is displayed # == Color == @@ -27,7 +27,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au # 7 Normal # 6 lane departure place hands on wheel - color = 2 if lkas_active else 1 + color = 2 if lkas_active else 1 if mads_enabled and not lkas_active else 0 lines = 3 if lkas_active else 0 alerts = 7 if lkas_active else 0 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index c8504d6fb1..219f0244a4 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -5,6 +5,10 @@ from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter + class CarInterface(CarInterfaceBase): @staticmethod @@ -85,8 +89,52 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) + buttonEvents = [] + + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): + if self.CS.out.cruiseState.enabled: # CANCEL + if not ret.cruiseState.enabled: + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + if self.CP.pcmCruise and self.CP.minEnableSpeed > 0: + if ret.gasPressed and not ret.cruiseState.enabled: + self.CS.accEnabled = False + self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # CANCEL + if 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) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + # events - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5): diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b4a79d10a6..be606c4021 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -99,12 +99,12 @@ class CarController: friction_brake_bus = CanBus.POWERTRAIN # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled and CS.out.cruiseState.enabled, at_full_stop)) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, idx, CC.enabled, near_stop, at_full_stop, self.CP)) # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled and CS.out.cruiseState.enabled, hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index de0fd2eed6..233a280940 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -21,6 +21,9 @@ class CarState(CarStateBase): self.camera_lka_steering_cmd_counter = 0 self.buttons_counter = 0 + self.lkas_enabled = False + self.prev_lkas_enabled = False + def update(self, pt_cp, cam_cp, loopback_cp): ret = car.CarState.new_message() @@ -29,6 +32,8 @@ class CarState(CarStateBase): self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0 + self.prev_mads_enabled = self.mads_enabled + self.prev_lkas_enabled = self.lkas_enabled # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 @@ -46,6 +51,8 @@ class CarState(CarStateBase): # sample rear wheel speeds, standstill=True if ECM allows engagement with brake ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD + self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"] + if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: ret.gearShifter = self.parse_gear_shifter("T") else: @@ -87,8 +94,8 @@ class CarState(CarStateBase): # 1 - latched ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0 - ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 - ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 + ret.leftBlinker = ret.leftBlinkerOn = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 + ret.rightBlinker = ret.rightBlinkerOn = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1 ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 @@ -140,6 +147,7 @@ class CarState(CarStateBase): ("AcceleratorPedal2", "AcceleratorPedal2"), ("CruiseState", "AcceleratorPedal2"), ("ACCButtons", "ASCMSteeringButton"), + ("LKAButton", "ASCMSteeringButton", 0), ("RollingCounter", "ASCMSteeringButton"), ("SteeringWheelAngle", "PSCMSteeringAngle"), ("SteeringWheelRate", "PSCMSteeringAngle"), diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 856dcbaae5..668ba37d19 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -205,21 +205,65 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) + buttonEvents = [] + if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: - buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)] + buttonEvents.append(create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)) # Handle ACCButtons changing buttons mid-press if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS: buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)) - ret.buttonEvents = buttonEvents + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + if not self.CP.pcmCruise: + if any(b.type == ButtonType.accelCruise and b.pressed for b in buttonEvents): + self.CS.accEnabled = True + + self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled, + buttonEvents, c.vCruise) + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + if self.CS.prev_lkas_enabled != 1 and self.CS.lkas_enabled == 1: + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): + if any(b.type == ButtonType.cancel for b in buttonEvents): + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + if self.CP.pcmCruise and self.CP.minEnableSpeed > 0: + if ret.gasPressed and not ret.cruiseState.enabled: + self.CS.accEnabled = False + self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents # The ECM allows enabling on falling edge of set, but only rising edge of resume events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.eco, GearShifter.manumatic], - pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,)) - if not self.CP.pcmCruise: - if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents): - events.add(EventName.buttonEnable) + pcm_enable=False, enable_buttons=(ButtonType.decelCruise,)) + #if not self.CP.pcmCruise: + # if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents): + # events.add(EventName.buttonEnable) + + events, ret = self.create_sp_events(self.CS, ret, events, enable_pressed=self.CS.accEnabled, + enable_buttons=(ButtonType.decelCruise,)) # Enabling at a standstill with brake is allowed # TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs @@ -229,7 +273,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.belowEngageSpeed) if ret.cruiseState.standstill: events.add(EventName.resumeRequired) - if ret.vEgo < self.CP.minSteerSpeed: + if ret.vEgo < self.CP.minSteerSpeed and self.CS.madsEnabled: events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index ab944a30aa..4565d7fccc 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -96,7 +96,7 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "lead_visible", - "lanes_visible", "fcw", "acc_alert", "steer_required"]) + "lanes_visible", "fcw", "acc_alert", "steer_required", "dashed_lanes"]) def rate_limit_steer(new_steer, last_steer): @@ -217,7 +217,7 @@ class CarController: self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) stopping = actuators.longControlState == LongCtrlState.stopping - can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas, + can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, CC.longActive, self.accel, self.gas, stopping, self.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) @@ -247,8 +247,8 @@ class CarController: # Send dashboard UI commands. if self.frame % 10 == 0: hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, - hud_control.lanesVisible, fcw_display, acc_alert, steer_required) - can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud)) + hud_control.lanesVisible, fcw_display, acc_alert, steer_required, CS.madsEnabled and not CC.latActive) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive)) if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: self.speed = pcm_speed diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 16880d1b1f..b36a3e71db 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -165,6 +165,7 @@ class CarState(CarStateBase): # update prevs, update must run once per loop self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_setting = self.cruise_setting + self.prev_mads_enabled = self.mads_enabled self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"] @@ -216,7 +217,7 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"] ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"] - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk( + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk( 250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"]) ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1 diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 17681444af..005ad5fb45 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -102,7 +102,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint): return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) -def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): +def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active): commands = [] bus_pt = get_pt_bus(CP.carFingerprint) radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl @@ -135,7 +135,8 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud_values = { 'SET_ME_X41': 0x41, 'STEERING_REQUIRED': hud.steer_required, - 'SOLID_LANES': hud.lanes_visible, + 'SOLID_LANES': lat_active, + 'DASHED_LANES': hud.dashed_lanes, 'BEEP': 0, } diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index da921ccda7..0c7d6238fa 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -11,6 +11,7 @@ from selfdrive.car.disable_ecu import disable_ecu ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} @@ -317,28 +318,59 @@ class CarInterface(CarInterfaceBase): if self.CS.cruise_setting != self.CS.prev_cruise_setting: buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1})) + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled, + buttonEvents, c.vCruise) + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + if self.CS.prev_cruise_setting != 1 and self.CS.cruise_setting == 1: + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): + if any(b.type == ButtonType.cancel for b in buttonEvents): + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + if self.CP.pcmCruise and self.CP.minEnableSpeed > 0: + if ret.gasPressed and not ret.cruiseState.enabled: + self.CS.accEnabled = False + self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + ret.buttonEvents = buttonEvents # events - events = self.create_common_events(ret, pcm_enable=False) + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) + if self.CS.brake_error: events.add(EventName.brakeUnavailable) - if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed: + if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed and not self.CS.madsEnabled: events.add(EventName.belowEngageSpeed) - if self.CP.pcmCruise: - # we engage when pcm is active (rising edge) - if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled: - events.add(EventName.pcmEnable) - elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl): - # it can happen that car cruise disables while comma system is enabled: need to - # keep braking if needed or if the speed is very low - if ret.vEgo < self.CP.minEnableSpeed + 2.: - # non loud alert if cruise disables below 25mph as expected (+ a little margin) - events.add(EventName.speedTooLow) - else: - events.add(EventName.cruiseDisabled) + #if self.CP.pcmCruise: + # # we engage when pcm is active (rising edge) + # if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled: + # events.add(EventName.pcmEnable) + # elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl): + # # it can happen that car cruise disables while comma system is enabled: need to + # # keep braking if needed or if the speed is very low + # if ret.vEgo < self.CP.minEnableSpeed + 2.: + # # non loud alert if cruise disables below 25mph as expected (+ a little margin) + # events.add(EventName.speedTooLow) + # else: + # events.add(EventName.cruiseDisabled) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.add(EventName.manualRestart) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b81c5e3f7d..c1ced274dd 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -54,6 +54,10 @@ class CarController: self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 + self.disengage_blink = 0. + self.lat_disengage_init = False + self.lat_active_last = False + def update(self, CC, CS): actuators = CC.actuators hud_control = CC.hudControl @@ -76,6 +80,20 @@ class CarController: sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, hud_control) + # show LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive + lateral_paused = CS.madsEnabled and not CC.latActive + if CC.latActive: + self.lat_disengage_init = False + elif self.lat_active_last: + self.lat_disengage_init = True + + if not self.lat_disengage_init: + self.disengage_blink = self.frame + + blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False + + self.lat_active_last = CC.latActive + can_sends = [] # *** common hyundai stuff *** @@ -112,7 +130,8 @@ class CarController: hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer, + lateral_paused, blinking_icon)) # disable LFA on HDA2 if self.frame % 5 == 0 and hda2: @@ -120,7 +139,8 @@ class CarController: # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, CC.latActive, + lateral_paused, blinking_icon)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: @@ -130,7 +150,7 @@ class CarController: if hda2: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units)) self.accel_last = accel else: @@ -159,7 +179,8 @@ class CarController: can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, - left_lane_warning, right_lane_warning)) + left_lane_warning, right_lane_warning, + lateral_paused, blinking_icon)) if not self.CP.openpilotLongitudinalControl: if CC.cruiseControl.cancel: @@ -174,15 +195,15 @@ class CarController: if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: # TODO: unclear if this is needed jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 - can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), - hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override)) + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2), + hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, CS.mainEnabled)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): - can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) + can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive, lateral_paused, blinking_icon)) # 5 Hz ACC options if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index da1a7bfa78..c6eb131b05 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -42,6 +42,10 @@ class CarState(CarStateBase): self.params = CarControllerParams(CP) + self.lfa_enabled = False + self.prev_lfa_enabled = False + self.mainEnabled = False + def update(self, cp, cp_cam): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam) @@ -82,7 +86,7 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"] ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"] ret.yawRate = cp.vl["ESP12"]["YAW_RATE"] - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp( + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_lamp( 50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"]) ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"] ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"] @@ -92,7 +96,7 @@ class CarState(CarStateBase): # cruise state if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons - ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 + ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 and self.mainEnabled ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False else: @@ -148,8 +152,18 @@ class CarState(CarStateBase): self.clu11 = copy.copy(cp.vl["CLU11"]) self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE self.prev_cruise_buttons = self.cruise_buttons[-1] + self.prev_main_buttons = self.main_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + if self.CP.openpilotLongitudinalControl: + if self.prev_main_buttons != 1: + if self.main_buttons[-1] == 1: + self.mainEnabled = not self.mainEnabled + ret.cruiseState.available = ret.cruiseState.available and self.mainEnabled + self.prev_mads_enabled = self.mads_enabled + self.prev_lfa_enabled = self.lfa_enabled + if self.CP.flags & HyundaiFlags.SP_CAN_LFA_BTN: + self.lfa_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] return ret @@ -194,8 +208,8 @@ class CarState(CarStateBase): ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], - cp.vl["BLINKERS"]["RIGHT_LAMP"]) + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_lamp( + 50, cp.vl["BLINKERS"]["LEFT_LAMP"], cp.vl["BLINKERS"]["RIGHT_LAMP"]) if self.CP.enableBsm: ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0 ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0 @@ -216,8 +230,12 @@ class CarState(CarStateBase): cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" self.prev_cruise_buttons = self.cruise_buttons[-1] + self.prev_main_buttons = self.main_buttons[-1] self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"]) self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"]) + self.prev_mads_enabled = self.mads_enabled + self.prev_lfa_enabled = self.lfa_enabled + self.lfa_enabled = cp.vl[cruise_btn_msg]["LFA_BTN"] self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"] ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED @@ -363,6 +381,10 @@ class CarState(CarStateBase): signals.append(("CF_Lvr_Gear", "LVR12")) checks.append(("LVR12", 100)) + if CP.flags & HyundaiFlags.SP_CAN_LFA_BTN: + signals.append(("LFA_Pressed", "BCM_PO_11")) + checks.append(("BCM_PO_11", 50)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod @@ -448,6 +470,7 @@ class CarState(CarStateBase): ("CRUISE_BUTTONS", cruise_btn_msg), ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg), ("DISTANCE_UNIT", "CRUISE_BUTTONS_ALT"), + ("LFA_BTN", cruise_btn_msg), ("LEFT_LAMP", "BLINKERS"), ("RIGHT_LAMP", "BLINKERS"), diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index c2ffffbf22..42c032293e 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -6,7 +6,8 @@ hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, torque_fault, lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, - left_lane_depart, right_lane_depart): + left_lane_depart, right_lane_depart, + lateral_paused, blinking_icon): values = lkas11 values["CF_Lkas_LdwsSysState"] = sys_state values["CF_Lkas_SysWarning"] = 3 if sys_warning else 0 @@ -31,7 +32,8 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, # FcwOpt_USM 2 = Green car + lanes # FcwOpt_USM 1 = White car + lanes # FcwOpt_USM 0 = No car + lanes - values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1 + values["CF_Lkas_FcwOpt_USM"] = 2 if steer_req else 2 if blinking_icon else 1 if\ + lateral_paused else 1 # SysWarning 4 = keep hands on wheel # SysWarning 5 = keep hands on wheel (red) @@ -48,7 +50,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, # SysState 1-2 = white car + lanes # SysState 3 = green car + lanes, green steering wheel # SysState 4 = green car + lanes - values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1 + values["CF_Lkas_LdwsSysState"] = 3 if steer_req else 1 values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition # these have no effect @@ -87,20 +89,20 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint): return packer.make_can_msg("CLU11", bus, values) -def create_lfahda_mfc(packer, enabled, hda_set_speed=0): +def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon, hda_set_speed=0): values = { - "LFA_Icon_State": 2 if enabled else 0, + "LFA_Icon_State": 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0, "HDA_Active": 1 if hda_set_speed else 0, "HDA_Icon_State": 2 if hda_set_speed else 0, "HDA_VSetReq": hda_set_speed, } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, main_enabled): commands = [] scc11_values = { - "MainMode_ACC": 1, + "MainMode_ACC": 1 if main_enabled else 0, "TauGapSet": 4, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index af7239571c..f81120c3c4 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -8,13 +8,13 @@ def get_e_can_bus(CP): return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 -def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): +def create_steering_messages(packer, CP, enabled, lat_active, apply_steer, lateral_paused, blinking_icon): ret = [] values = { "LKA_MODE": 2, - "LKA_ICON": 2 if enabled else 1, + "LKA_ICON": 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0, "TORQUE_REQUEST": apply_steer, "LKA_ASSIST": 0, "STEER_REQ": 1 if lat_active else 0, @@ -56,10 +56,10 @@ def create_acc_cancel(packer, CP, cruise_info_copy): }) return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) -def create_lfahda_cluster(packer, CP, enabled): +def create_lfahda_cluster(packer, CP, enabled, lat_active, lateral_paused, blinking_icon): values = { "HDA_ICON": 1 if enabled else 0, - "LFA_ICON": 2 if enabled else 0, + "LFA_ICON": 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0, } return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6d6d9833df..8034be0f86 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -11,6 +11,7 @@ from selfdrive.car.disable_ecu import disable_ecu Ecu = car.CarParams.Ecu ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} @@ -267,6 +268,10 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_SCC_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + if 0x391 in fingerprint[0]: + ret.flags |= HyundaiFlags.SP_CAN_LFA_BTN.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN + if ret.openpilotLongitudinalControl: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG if candidate in HYBRID_CAR: @@ -301,26 +306,61 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: - buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)] + buttonEvents = [] + + if self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: + buttonEvents.append(create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)) # Handle CF_Clu_CruiseSwState changing buttons mid-press if self.CS.cruise_buttons[-1] != 0 and self.CS.prev_cruise_buttons != 0: buttonEvents.append(create_button_event(0, self.CS.prev_cruise_buttons, BUTTONS_DICT)) - ret.buttonEvents = buttonEvents + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled, + buttonEvents, c.vCruise) + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + if self.CS.prev_lfa_enabled != 1 and self.CS.lfa_enabled == 1: + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if not self.CP.pcmCruise: + if any(b.type == ButtonType.cancel for b in buttonEvents): + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons # Main button also can trigger an engagement on these cars allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons) - events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.manumatic], + pcm_enable=False, allow_enable=allow_enable) + + events, ret = self.create_sp_events(self.CS, ret, events, main_enabled=True, allow_enable=allow_enable) # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: self.low_speed_alert = True if ret.vEgo > (self.CP.minSteerSpeed + 4.): self.low_speed_alert = False - if self.low_speed_alert: + if self.low_speed_alert and self.CS.madsEnabled: events.add(car.CarEvent.EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index c0cf23b50d..6cc4b54803 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -61,6 +61,8 @@ class HyundaiFlags(IntFlag): ENABLE_BLINKERS = 32 + SP_CAN_LFA_BTN = 64 + class CAR: # Hyundai diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7192f5252c..7f56531933 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -9,9 +9,11 @@ from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D from common.numpy_fast import clip, interp +from common.params import Params from common.realtime import DT_CTRL from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone +from selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN +from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, V_CRUISE_MAX, apply_center_deadzone from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -83,6 +85,17 @@ class CarInterfaceBase(ABC): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM) + self.param_s = Params() + self.disengage_on_accelerator = self.param_s.get_bool("DisengageOnAccelerator") + self.enable_mads = self.param_s.get_bool("EnableMads") + self.mads_disengage_lateral_on_brake = self.param_s.get_bool("DisengageLateralOnBrake") + self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake + self.gear_warning = 0 + self.cruise_cancelled_btn = True + self.acc_mads_combo = self.param_s.get_bool("AccMadsCombo") + self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause") + self.prev_acc_mads_combo = False + @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @@ -242,13 +255,19 @@ class CarInterfaceBase(ABC): if cs_out.doorOpen: events.add(EventName.doorOpen) - if cs_out.seatbeltUnlatched: + if cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park: events.add(EventName.seatbeltNotLatched) - if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or - cs_out.gearShifter not in extra_gears): - events.add(EventName.wrongGear) + if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears and not \ + (cs_out.gearShifter == GearShifter.unknown and self.gear_warning < int(0.5/DT_CTRL)): + if cs_out.vEgo < 5: + events.add(EventName.silentWrongGear) + else: + events.add(EventName.wrongGear) if cs_out.gearShifter == GearShifter.reverse: - events.add(EventName.reverseGear) + if cs_out.vEgo < 5: + events.add(EventName.spReverseGear) + else: + events.add(EventName.reverseGear) if not cs_out.cruiseState.available: events.add(EventName.wrongCarMode) if cs_out.espDisabled: @@ -262,7 +281,12 @@ class CarInterfaceBase(ABC): if cs_out.cruiseState.nonAdaptive: events.add(EventName.wrongCruiseMode) if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl: - events.add(EventName.brakeHold) + if cs_out.madsEnabled: + cs_out.disengageByBrake = True + if cs_out.cruiseState.enabled: + events.add(EventName.brakeHold) + else: + events.add(EventName.silentBrakeHold) if cs_out.parkingBrake: events.add(EventName.parkBrake) if cs_out.accFaulted: @@ -270,14 +294,16 @@ class CarInterfaceBase(ABC): if cs_out.steeringPressed: events.add(EventName.steerOverride) + self.gear_warning = self.gear_warning + 1 if cs_out.gearShifter == GearShifter.unknown else 0 + # Handle button presses - for b in cs_out.buttonEvents: - # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port) - if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed): - events.add(EventName.buttonEnable) - # Disable on rising and falling edge of cancel for both stock and OP long - if b.type == ButtonType.cancel: - events.add(EventName.buttonCancel) + #for b in cs_out.buttonEvents: + # # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port) + # if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed): + # events.add(EventName.buttonEnable) + # # Disable on rising and falling edge of cancel for both stock and OP long + # if b.type == ButtonType.cancel: + # events.add(EventName.buttonCancel) # Handle permanent and temporary steering faults self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1 @@ -293,6 +319,12 @@ class CarInterfaceBase(ABC): if cs_out.steerFaultPermanent: events.add(EventName.steerUnavailable) + # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. + if (cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator) or \ + (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill) and not self.mads_ndlob): + if cs_out.madsEnabled: + cs_out.disengageByBrake = True + # we engage when pcm is active (rising edge) # enabling can optionally be blocked by the car interface if pcm_enable: @@ -303,6 +335,128 @@ class CarInterfaceBase(ABC): return events + @staticmethod + def sp_v_cruise_initialized(v_cruise): + return v_cruise != V_CRUISE_INITIAL + + def get_acc_mads(self, cruiseState_enabled, acc_enabled, mads_enabled): + if self.acc_mads_combo: + if not self.prev_acc_mads_combo and (cruiseState_enabled or acc_enabled): + mads_enabled = True + self.prev_acc_mads_combo = (cruiseState_enabled or acc_enabled) + + return mads_enabled + + def get_sp_v_cruise_non_pcm_state(self, cruiseState_available, acc_enabled, button_events, vCruise, + enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): + + if cruiseState_available: + for b in button_events: + if not self.CP.pcmCruise: + if b.type in enable_buttons and not b.pressed: + acc_enabled = True + if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and not self.sp_v_cruise_initialized(vCruise): + acc_enabled = False + else: + acc_enabled = False + + return acc_enabled, button_events + + def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False): + mads_enabled = False if not self.enable_mads else mads_enabled + return mads_enabled, acc_enabled + + def get_sp_pedal_disengage(self, brake_pressed, standstill): + return brake_pressed and (not self.CS.out.brakePressed or not standstill) + + def get_sp_common_state(self, cs_out, CS, controls_cancel, gear_allowed=True): + if self.CP.pcmCruise: + if not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled: + CS.madsEnabled, cs_out.cruiseState.enabled = self.get_sp_cancel_cruise_state(CS.madsEnabled) + + CS.accEnabled = False if controls_cancel else CS.accEnabled + cs_out.cruiseState.enabled = cs_out.cruiseState.enabled if self.CP.pcmCruise else CS.accEnabled + if not self.enable_mads: + if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled: + CS.madsEnabled = True + elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled: + CS.madsEnabled = False + + cs_out.belowLaneChangeSpeed = cs_out.vEgo < LANE_CHANGE_SPEED_MIN and self.below_speed_pause + + if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \ + (cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park): + gear_allowed = False + + cs_out.latActive = gear_allowed + + if not CS.control_initialized: + CS.control_initialized = True + + cs_out.madsEnabled = CS.madsEnabled + cs_out.accEnabled = CS.accEnabled + + return cs_out, CS + + def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False, + enable_from_brake=False, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): + + CS.disengageByBrake = CS.disengageByBrake or cs_out.disengageByBrake + + if CS.disengageByBrake and not cs_out.brakePressed and not cs_out.brakeHoldActive and not cs_out.parkingBrake and cs_out.madsEnabled: + enable_pressed = True + enable_from_brake = True + + if not cs_out.brakePressed and not cs_out.brakeHoldActive and not cs_out.parkingBrake: + CS.disengageByBrake = False + cs_out.disengageByBrake = False + + for b in cs_out.buttonEvents: + # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port) + if not self.CP.pcmCruise: + if b.type in enable_buttons and not b.pressed: + enable_pressed = True + # Disable on rising and falling edge of cancel for both stock and OP long + if b.type == ButtonType.cancel: + if not cs_out.madsEnabled: + events.add(EventName.buttonCancel) + elif not self.cruise_cancelled_btn: + self.cruise_cancelled_btn = True + events.add(EventName.manualLongitudinalRequired) + # do disable on MADS button if ACC is disabled + if b.type == ButtonType.altButton1 and b.pressed: + if not cs_out.madsEnabled: # disabled MADS + if not cs_out.cruiseState.enabled: + events.add(EventName.buttonCancel) + else: + events.add(EventName.manualSteeringRequired) + else: # enabled MADS + if not cs_out.cruiseState.enabled: + enable_pressed = True + if self.CP.pcmCruise: + # do disable on button down + if main_enabled: + if any(CS.main_buttons) and not cs_out.cruiseState.enabled: + if not cs_out.madsEnabled: + events.add(EventName.buttonCancel) + # do enable on both accel and decel buttons + if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled and allow_enable: + enable_pressed = True + elif not cs_out.cruiseState.enabled: + if not cs_out.madsEnabled: + events.add(EventName.buttonCancel) + elif not self.enable_mads: + cs_out.madsEnabled = False + if enable_pressed: + if enable_from_brake: + events.add(EventName.silentButtonEnable) + else: + events.add(EventName.buttonEnable) + + if cs_out.cruiseState.enabled: + self.cruise_cancelled_btn = False + + return events, cs_out class RadarInterfaceBase(ABC): def __init__(self, CP): @@ -334,6 +488,13 @@ class CarStateBase(ABC): self.cluster_speed_hyst_gap = 0.0 self.cluster_min_speed = 0.0 # min speed before dropping to 0 + self.accEnabled = False + self.madsEnabled = False + self.disengageByBrake = False + self.mads_enabled = False + self.prev_mads_enabled = False + self.control_initialized = False + # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) # R = 0.3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], @@ -429,7 +590,6 @@ class CarStateBase(ABC): def get_loopback_can_parser(CP): return None - # interface-specific helpers def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str, Any]: diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 944d79809b..0d1fbe8181 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -18,9 +18,16 @@ class CarState(CarStateBase): self.lkas_allowed_speed = False self.lkas_disabled = False + self.lkas_enabled = False + self.prev_lkas_enabled = False + def update(self, cp, cp_cam): ret = car.CarState.new_message() + + self.prev_mads_enabled = self.mads_enabled + self.prev_lkas_enabled = self.lkas_enabled + ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], cp.vl["WHEEL_SPEEDS"]["FR"], @@ -34,14 +41,16 @@ class CarState(CarStateBase): speed_kph = cp.vl["ENGINE_DATA"]["SPEED"] ret.standstill = speed_kph < .1 + self.lkas_enabled = not self.lkas_disabled + can_gear = int(cp.vl["GEAR"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"]) ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1 ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1 - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, - cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, + cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fdd2439ff9..f4e63c0a32 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -7,6 +7,7 @@ from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @@ -57,12 +58,51 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - # events - events = self.create_common_events(ret) + buttonEvents = [] - if self.CS.lkas_disabled: - events.add(EventName.lkasDisabled) - elif self.CS.low_speed_alert: + # CANCEL + if 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) + + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + if self.CS.prev_lkas_enabled != self.CS.lkas_enabled: + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \ + self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + + # events + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], + pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) + + #if self.CS.lkas_disabled: + # events.add(EventName.lkasDisabled) + if self.CS.low_speed_alert: events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index ff13812398..0f61d937f2 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,4 +1,5 @@ from cereal import car +from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.nissan import nissancan @@ -18,11 +19,28 @@ class CarController: self.packer = CANPacker(dbc_name) + self.disengage_blink = 0. + self.lat_disengage_init = False + self.lat_active_last = False + def update(self, CC, CS): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel + lateral_paused = CS.madsEnabled and not CC.latActive + if CC.latActive: + self.lat_disengage_init = False + elif self.lat_active_last: + self.lat_disengage_init = True + + if not self.lat_disengage_init: + self.disengage_blink = self.frame + + blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False + + self.lat_active_last = CC.latActive + can_sends = [] ### STEER ### @@ -63,12 +81,12 @@ class CarController: can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) can_sends.append(nissancan.create_steering_control( - self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) + self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque)) if lkas_hud_msg and lkas_hud_info_msg: if self.frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + self.packer, lkas_hud_msg, CC.latActive, blinking_icon, lateral_paused, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) if self.frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index d6b6d17d55..325bf5bcd4 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -23,6 +23,8 @@ class CarState(CarStateBase): def update(self, cp, cp_adas, cp_cam): ret = car.CarState.new_message() + self.prev_mads_enabled = self.mads_enabled + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): @@ -88,8 +90,8 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] - ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) - ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) + ret.leftBlinker = ret.leftBlinkerOn = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) + ret.rightBlinker = ret.rightBlinkerOn = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"], cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"], diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 386e859089..b67d1e2b4a 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -4,6 +4,9 @@ from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.nissan.values import CAR +ButtonType = car.CarState.ButtonEvent.Type +GearShifter = car.CarState.GearShifter + class CarInterface(CarInterfaceBase): @@ -43,11 +46,46 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) buttonEvents = [] - be = car.CarState.ButtonEvent.new_message() - be.type = car.CarState.ButtonEvent.Type.accelCruise - buttonEvents.append(be) + #be = car.CarState.ButtonEvent.new_message() + #be.type = car.CarState.ButtonEvent.Type.accelCruise + #buttonEvents.append(be) - events = self.create_common_events(ret) + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \ + self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # CANCEL + if 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) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], + pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) if self.CS.lkas_enabled: events.add(car.CarEvent.EventName.invalidLkasSetting) diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index 01fb3463a9..db8f3cca31 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -45,15 +45,15 @@ def create_cancel_msg(packer, cancel_msg, cruise_cancel): return packer.make_can_msg("CANCEL_MSG", 2, values) -def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart): +def create_lkas_hud_msg(packer, lkas_hud_msg, lat_active, blinking_icon, lateral_paused, left_line, right_line, left_lane_depart, right_lane_depart): values = lkas_hud_msg values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0 values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0 - values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0 - values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0 - values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0 + values["LARGE_STEERING_WHEEL_ICON"] = 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0, + values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0 + values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active else 0 return packer.make_can_msg("PROPILOT_HUD", 0, values) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a56e63408e..6e2949e28c 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -80,7 +80,7 @@ class CarController: self.es_dashstatus_cnt = CS.es_dashstatus_msg["COUNTER"] if self.es_lkas_cnt != CS.es_lkas_msg["COUNTER"]: - can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert, + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.latActive, CS.madsEnabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"] diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ba873c48d7..4d70e835cf 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -13,9 +13,15 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["Transmission"]["Gear"] + self.lkas_enabled = None + self.prev_lkas_enabled = None + def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() + self.prev_mads_enabled = self.mads_enabled + self.prev_lkas_enabled = self.lkas_enabled + ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255. ret.gasPressed = ret.gas > 1e-5 if self.car_fingerprint in PREGLOBAL_CARS: @@ -35,9 +41,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 + if self.car_fingerprint not in PREGLOBAL_CARS: + self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"] + if self.prev_lkas_enabled is None: + self.prev_lkas_enabled = self.lkas_enabled + # continuous blinker signals for assisted lane change - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"], - cp.vl["Dashlights"]["RIGHT_BLINKER"]) + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"], + cp.vl["Dashlights"]["RIGHT_BLINKER"]) if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1) @@ -62,6 +73,11 @@ class CarState(CarStateBase): (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): ret.cruiseState.speed *= CV.MPH_TO_KPH + self.madsEnabled, ret.cruiseState.enabled = self.update_sp_state(ret.cruiseState.enabled, self.enable_mads, + self.accEnabled, self.prev_cruiseState_enabled, + self.madsEnabled) + self.prev_brake_pressed = ret.brakePressed + ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1 ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"], cp.vl["BodyInfo"]["DOOR_OPEN_RL"], diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 22468801ec..22f00e242a 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -5,6 +5,10 @@ from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter + class CarInterface(CarInterfaceBase): @@ -108,7 +112,53 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) - ret.events = self.create_common_events(ret).to_msg() + buttonEvents = [] + + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + if self.CS.car_fingerprint not in PREGLOBAL_CARS: + if self.CS.prev_lkas_enabled != self.CS.lkas_enabled and self.CS.lkas_enabled != 2 and not (self.CS.prev_lkas_enabled == 2 and self.CS.lkas_enabled == 1): + self.CS.madsEnabled = not self.CS.madsEnabled + elif self.CS.prev_lkas_enabled != self.CS.lkas_enabled and self.CS.prev_lkas_enabled == 2 and self.CS.lkas_enabled != 1: + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if self.CS.out.cruiseState.enabled: # CANCEL + if not ret.cruiseState.enabled: + if not self.enable_mads: + self.CS.madsEnabled = False + if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # CANCEL + if 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) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) + + ret.events = events.to_msg() return ret diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index d83b639a41..7ea5ff79f0 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -21,7 +21,7 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): values["Cruise_Cancel"] = 1 return packer.make_can_msg("ES_Distance", bus, values) -def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): +def create_es_lkas(packer, es_lkas_msg, lat_active, mads_enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): values = copy.copy(es_lkas_msg) @@ -56,12 +56,17 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_ elif right_lane_depart: values["LKAS_Alert"] = 11 # Right lane departure dash alert - if enabled: + if lat_active: values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines values["LKAS_Dash_State"] = 2 # Green enabled indicator + elif mads_enabled and not lat_active: + values["LKAS_Dash_State"] = 1 # White ready indicator else: values["LKAS_Dash_State"] = 0 # LKAS Not enabled + values["LKAS_Left_Line_Enable"] = 1 if lat_active else 0 + values["LKAS_Right_Line_Enable"] = 1 if lat_active else 0 + values["LKAS_Left_Line_Visible"] = int(left_line) values["LKAS_Right_Line_Visible"] = int(right_line) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6dbdd4b5d9..e8ce1bfc80 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -78,7 +78,7 @@ class CarController: # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different - if not CC.enabled and CS.pcm_acc_status: + if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status: pcm_cancel_cmd = 1 # on entering standstill, send standstill request @@ -146,7 +146,7 @@ class CarController: if self.frame % 100 == 0 or send_ui: can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, - hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) + hud_control.rightLaneDepart, CC.latActive, CS.lkas_hud, CS.madsEnabled)) if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 050f8747a2..319f025a99 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -30,9 +30,19 @@ class CarState(CarStateBase): self.acc_type = 1 self.lkas_hud = {} + self.lkas_enabled = None + self.prev_lkas_enabled = None + self.lta_status = False + self.prev_lta_status = False + self.lta_status_active = False + def update(self, cp, cp_cam): ret = car.CarState.new_message() + self.prev_mads_enabled = self.mads_enabled + self.prev_lkas_enabled = self.lkas_enabled + self.prev_lta_status = self.lta_status + 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"]]) ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0 @@ -61,6 +71,20 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw == 0 + if self.CP.carFingerprint != CAR.PRIUS_V: + self.lta_status = cp_cam.vl["LKAS_HUD"]["SET_ME_X02"] + if ((self.prev_lta_status == 16 and self.lta_status == 0) or + (self.prev_lta_status == 0 and self.lta_status == 16)) and not self.lta_status_active: + self.lta_status_active = True + if self.prev_lta_status is None: + self.prev_lta_status = self.lta_status + if self.lta_status_active: + self.lkas_enabled = self.lta_status + elif self.CP.carFingerprint != CAR.PRIUS_V: + self.lkas_enabled = cp_cam.vl["LKAS_HUD"]["LKAS_STATUS"] + if self.prev_lkas_enabled is None: + self.prev_lkas_enabled = self.lkas_enabled + ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] @@ -81,8 +105,8 @@ class CarState(CarStateBase): 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 - ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 + ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 + ret.rightBlinker = ret.rightBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale @@ -267,6 +291,8 @@ class CarState(CarStateBase): ("LANE_SWAY_WARNING", "LKAS_HUD"), ("LANE_SWAY_SENSITIVITY", "LKAS_HUD"), ("LANE_SWAY_TOGGLE", "LKAS_HUD"), + ("LKAS_STATUS", "LKAS_HUD"), + ("SET_ME_X02", "LKAS_HUD"), ] checks += [ ("LKAS_HUD", 1), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8e180e2301..29a38ca07a 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,10 +3,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 import STD_CARGO_KG, scale_tire_stiffness, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @@ -240,8 +242,53 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) + buttonEvents = [] + + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + if ret.cruiseState.available: + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled: + self.CS.madsEnabled = True + if self.CS.lta_status_active: + if (self.CS.prev_lkas_enabled == 16 and self.CS.lkas_enabled == 0) or \ + (self.CS.prev_lkas_enabled == 0 and self.CS.lkas_enabled == 16): + self.CS.madsEnabled = not self.CS.madsEnabled + else: + if (not self.CS.prev_lkas_enabled and self.CS.lkas_enabled) or \ + (self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled): + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.CS.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + else: + self.CS.madsEnabled = False + + if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \ + self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # CANCEL + if 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) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + # events - events = self.create_common_events(ret) + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], + pcm_enable=False) + + events, ret = self.create_sp_events(self.CS, ret, events) if self.CP.openpilotLongitudinalControl: if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7e360cc4e1..4529eb517b 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -66,18 +66,20 @@ def create_fcw_command(packer, fcw): return packer.make_can_msg("ACC_HUD", 0, values) -def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud): +def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, lat_active, stock_lkas_hud, + mads_enabled): + lateral_paused = mads_enabled and not lat_active values = { "TWO_BEEPS": chime, - "LDA_ALERT": steer, - "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, - "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, - "BARRIERS": 1 if enabled else 0, + "LDA_ALERT": steer if mads_enabled else 0, + "RIGHT_LINE": 0 if not mads_enabled else 2 if lateral_paused else 3 if right_lane_depart else 1 if right_line else 2, + "LEFT_LINE": 0 if not mads_enabled else 2 if lateral_paused else 3 if left_lane_depart else 1 if left_line else 2, + "BARRIERS": 1 if lat_active else 0, + "LKAS_STATUS": 2 if mads_enabled else 1 if lateral_paused else 0, # static signals "SET_ME_X02": 2, "SET_ME_X01": 1, - "LKAS_STATUS": 1, "REPEATED_BEEPS": 0, "LANE_SWAY_FLD": 7, "LANE_SWAY_BUZZER": 0, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4b19f4d13c..39b7fc6d71 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -85,7 +85,7 @@ class CarController: hud_alert = 0 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw): hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"] - can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, + can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, CC.latActive, CS.out.steeringPressed, hud_alert, hud_control)) if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 64d1246880..17e318c242 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -34,6 +34,9 @@ class CarState(CarStateBase): return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type) ret = car.CarState.new_message() + + self.prev_mads_enabled = self.mads_enabled + # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"], @@ -134,8 +137,8 @@ class CarState(CarStateBase): ret.cruiseState.speed = 0 # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough - ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) - ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) + ret.leftBlinker = ret.leftBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) + ret.rightBlinker = ret.rightBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) self.gra_stock_values = pt_cp.vl["GRA_ACC_01"] @@ -149,6 +152,9 @@ class CarState(CarStateBase): def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): ret = car.CarState.new_message() + + self.prev_mads_enabled = self.mads_enabled + # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"], @@ -239,8 +245,8 @@ class CarState(CarStateBase): ret.cruiseState.speed = 0 # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"], - pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) + ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"], + pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) self.gra_stock_values = pt_cp.vl["GRA_Neu"] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index da0ce25afa..072adc1986 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -217,16 +217,45 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available + + self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled, + ret.buttonEvents, c.vCruise, + enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) + + if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): + if any(b.type == ButtonType.cancel for b in buttonEvents): + self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) + if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill): + self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled + + if self.CP.pcmCruise and self.CP.minEnableSpeed > 0: + if ret.gasPressed and not ret.cruiseState.enabled: + self.CS.accEnabled = False + self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled + + ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel) + + # MADS BUTTON + if self.CS.out.madsEnabled != self.CS.madsEnabled: + be = car.CarState.ButtonEvent.new_message() + be.pressed = True + be.type = ButtonType.altButton1 + ret.buttonEvents.append(be) + events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], - pcm_enable=not self.CS.CP.openpilotLongitudinalControl, + pcm_enable=False, enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) + events, ret = self.create_sp_events(self.CS, ret, events, + enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) + # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.): self.low_speed_alert = True elif ret.vEgo > (self.CP.minSteerSpeed + 2.): self.low_speed_alert = False - if self.low_speed_alert: + if self.low_speed_alert and self.CS.madsEnabled: events.add(EventName.belowSteerSpeed) if self.CS.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 30a51f6fe6..7d95658b67 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -13,12 +13,12 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled): return packer.make_can_msg("HCA_01", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, lat_active, steering_pressed, hud_alert, hud_control): values = ldw_stock_values.copy() values.update({ - "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0, - "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0, + "LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0, "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, "LDW_Texte": hud_alert, diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 130f107950..7c0eaec793 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -9,12 +9,12 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled): return packer.make_can_msg("HCA_1", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, lat_active, steering_pressed, hud_alert, hud_control): values = ldw_stock_values.copy() values.update({ - "LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0, - "LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lampe_gelb": 1 if lat_active and steering_pressed else 0, + "LDW_Lampe_gruen": 1 if lat_active and not steering_pressed else 0, "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, "LDW_Textbits": hud_alert, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21d8fe2d66..b9f5b2b15b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -108,9 +108,20 @@ class Controls: # set alternative experiences from parameters self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.enable_mads = self.params.get_bool("EnableMads") + self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake") + self.mads_dlob = self.enable_mads and self.mads_disengage_lateral_on_brake + self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake + if self.enable_mads and self.disengage_on_accelerator: + self.params.put_bool("DisengageOnAccelerator", False) + self.disengage_on_accelerator = False self.CP.alternativeExperience = 0 if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + if self.mads_dlob: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS + elif self.mads_ndlob: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE if self.CP.dashcamOnly and self.params.get_bool("DashcamOverride"): self.CP.dashcamOnly = False @@ -166,10 +177,12 @@ class Controls: self.initialized = False self.state = State.disabled self.enabled = False + self.enabled_long = False self.active = False self.can_rcv_timeout = False self.soft_disable_timer = 0 self.mismatch_counter = 0 + self.mismatch_counter_long = 0 self.cruise_mismatch_counter = 0 self.can_rcv_timeout_counter = 0 self.last_blinker_frame = 0 @@ -245,12 +258,15 @@ class Controls: if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): - self.events.add(EventName.pedalPressed) + if CS.cruiseState.enabled: + self.events.add(EventName.pedalPressed) + else: + self.events.add(EventName.silentPedalPressed) - if CS.brakePressed and CS.standstill: + if CS.brakePressed and CS.standstill and CS.cruiseState.enabled: self.events.add(EventName.preEnableStandstill) - if CS.gasPressed: + if CS.gasPressed and CS.cruiseState.enabled: self.events.add(EventName.gasPressedOverride) if not self.CP.notCar: @@ -317,6 +333,8 @@ class Controls: if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) + if self.mismatch_counter_long >= 200: + self.events.add(EventName.controlsMismatchLong) if log.PandaState.FaultType.relayMalfunction in pandaState.faults: self.events.add(EventName.relayMalfunction) @@ -383,7 +401,7 @@ class Controls: if not REPLAY: # Check for mismatch between openpilot and car's PCM - cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) + cruise_mismatch = CS.cruiseState.enabled and not self.enabled self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 if self.cruise_mismatch_counter > int(6. / DT_CTRL): self.events.add(EventName.cruiseMismatch) @@ -451,11 +469,16 @@ class Controls: # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not self.enabled: self.mismatch_counter = 0 + if not self.enabled_long: + self.mismatch_counter_long = 0 # All pandas not in silent mode must have controlsAllowed when openpilot is enabled if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 + if self.enabled_long and any(not ps.controlsAllowedLong for ps in self.sm['pandaStates'] + if ps.safetyModel not in IGNORED_SAFETY_MODES): + self.mismatch_counter_long += 1 self.distance_traveled += CS.vEgo * DT_CTRL @@ -464,7 +487,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) + self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -486,6 +509,12 @@ class Controls: else: # ENABLED if self.state == State.enabled: + if CS.cruiseState.enabled and not self.CS_prev.cruiseState.enabled: + self.v_cruise_helper.initialize_v_cruise(CS) + # Block resume if cruise never previously enabled + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: + self.current_alert_types.append(ET.NO_ENTRY) if self.events.any(ET.SOFT_DISABLE): self.state = State.softDisabling self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) @@ -539,10 +568,12 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS) + if CS.cruiseState.enabled: + self.v_cruise_helper.initialize_v_cruise(CS) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES + self.enabled_long = self.enabled and CS.cruiseState.enabled self.active = self.state in ACTIVE_STATES if self.active: self.current_alert_types.append(ET.WARNING) @@ -570,9 +601,11 @@ class Controls: # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill - CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ - (not standstill or self.joystick_mode) - CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl + CC.latActive = (self.active or self.mads_ndlob) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + (not standstill or self.joystick_mode) and CS.madsEnabled and (not CS.brakePressed or self.mads_ndlob) and \ + (not CS.belowLaneChangeSpeed or (not (((self.sm.frame - self.last_blinker_frame) * DT_CTRL) < 1.0) and + not (CS.leftBlinker or CS.rightBlinker))) and CS.latActive + CC.longActive = self.enabled_long and not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and not self.events.any(ET.OVERRIDE_LONGITUDINAL) actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -624,14 +657,14 @@ class Controls: lac_log.saturated = abs(actuators.steer) >= 0.9 # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: + if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode and CS.madsEnabled: undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 turning = abs(lac_log.desiredLateralAccel) > 1.0 good_speed = CS.vEgo > 5 max_torque = abs(self.last_actuators.steer) > 0.99 if undershooting and turning and good_speed and max_torque: self.events.add(EventName.steerSaturated) - elif lac_log.active and lac_log.saturated: + elif lac_log.active and lac_log.saturated and CS.madsEnabled: dpath_points = lat_plan.dPathPoints if len(dpath_points): # Check if we deviated from the path @@ -671,18 +704,19 @@ class Controls: if len(angular_rate_value) > 2: CC.angularVelocity = angular_rate_value - CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl - CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) + CC.cruiseControl.override = self.enabled_long and not CC.longActive and self.CP.openpilotLongitudinalControl + CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled_long or (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill))) if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: CC.cruiseControl.cancel = True speeds = self.sm['longitudinalPlan'].speeds if len(speeds): CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 + CC.vCruise = float(self.v_cruise_helper.v_cruise_kph) hudControl = CC.hudControl hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) - hudControl.speedVisible = self.enabled + hudControl.speedVisible = self.enabled_long hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead @@ -714,7 +748,7 @@ class Controls: clear_event_types = set() if ET.WARNING not in self.current_alert_types: clear_event_types.add(ET.WARNING) - if self.enabled: + if self.enabled and (self.CP.pcmCruise or (not self.CP.pcmCruise and self.v_cruise_helper.v_cruise_initialized)): clear_event_types.add(ET.NO_ENTRY) alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer]) @@ -758,8 +792,8 @@ class Controls: controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] - controlsState.enabled = self.enabled - controlsState.active = self.active + controlsState.enabled = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.enabled or CS.cruiseState.enabled) + controlsState.active = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.active or CS.cruiseState.enabled) controlsState.curvature = curvature controlsState.desiredCurvature = self.desired_curvature controlsState.desiredCurvatureRate = self.desired_curvature_rate diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 4790b8f0eb..342f343986 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -45,12 +45,12 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: + if not carstate.madsEnabled or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: # LaneChangeState.off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed and not carstate.brakePressed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 3036828662..1fe09b7302 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -542,6 +542,22 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), }, + EventName.manualSteeringRequired: { + ET.WARNING: Alert( + "Automatic Lane Centering is OFF", + "Manual Steering Required", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.), + }, + + EventName.manualLongitudinalRequired: { + ET.WARNING: Alert( + "Smart/Adaptive Cruise Control is OFF", + "Manual Gas/Brakes Required", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), + }, + EventName.steerSaturated: { ET.WARNING: Alert( "Take Control", @@ -590,6 +606,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.ENABLE: EngagementAlert(AudibleAlert.engage), }, + EventName.silentButtonEnable: { + ET.ENABLE: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.none, .2, 0., 0.), + }, + EventName.pcmDisable: { ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), }, @@ -604,6 +628,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"), }, + EventName.silentBrakeHold: { + ET.USER_DISABLE: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.none, .2, 0., 0.), + ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"), + }, + EventName.parkBrake: { ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"), @@ -615,6 +648,16 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { visual_alert=VisualAlert.brakePressed), }, + EventName.silentPedalPressed: { + ET.USER_DISABLE: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.none, .2), + ET.NO_ENTRY: NoEntryAlert("Pedal Pressed During Attempt", + visual_alert=VisualAlert.brakePressed), + }, + EventName.preEnableStandstill: { ET.PRE_ENABLE: Alert( "Release Brake to Engage", @@ -701,6 +744,19 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Gear not D"), }, + EventName.silentWrongGear: { + ET.SOFT_DISABLE: Alert( + "Gear not D", + "openpilot Unavailable", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 2., 3.), + ET.NO_ENTRY: Alert( + "Gear not D", + "openpilot Unavailable", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 2., 3.), + }, + # This alert is thrown when the calibration angles are outside of the acceptable range. # For example if the device is pointed too much to the left or the right. # Usually this can only be solved by removing the mount from the windshield completely, @@ -820,6 +876,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), }, + EventName.controlsMismatchLong: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch\nLongitudinal"), + ET.NO_ENTRY: NoEntryAlert("Controls Mismatch\nLongitudinal"), + }, + EventName.roadCameraError: { ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road", duration=1., @@ -892,6 +953,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), }, + EventName.spReverseGear: { + ET.PERMANENT: Alert( + "Reverse\nGear", + "", + AlertStatus.normal, AlertSize.full, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), + ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), + }, + # On cars that use stock ACC the car can decide to cancel ACC for various reasons. # When this happens we can no long control the car so the user needs to be warned immediately. EventName.cruiseDisabled: { diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 865966d6c5..1b8503c221 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -36,11 +36,16 @@ def manager_init() -> None: default_params: List[Tuple[str, Union[str, bytes]]] = [ ("CompletedTrainingVersion", "0"), - ("DisengageOnAccelerator", "1"), + ("DisengageOnAccelerator", "0"), ("GsmMetered", "1"), ("HasAcceptedTerms", "0"), ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), + + ("AccMadsCombo", "1"), + ("BelowSpeedPause", "1"), + ("DisengageLateralOnBrake", "1"), + ("EnableMads", "1"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8'))) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4df963167a..d485583a14 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -178,6 +178,8 @@ void ui_update_params(UIState *s) { void UIState::updateStatus() { if (scene.started && sm->updated("controlsState")) { auto controls_state = (*sm)["controlsState"].getControlsState(); + auto car_control = (*sm)["carControl"].getCarControl(); + auto car_state = (*sm)["carState"].getCarState(); auto alert_status = controls_state.getAlertStatus(); auto state = controls_state.getState(); if (alert_status == cereal::ControlsState::AlertStatus::USER_PROMPT) { @@ -187,7 +189,7 @@ void UIState::updateStatus() { } else if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) { status = STATUS_OVERRIDE; } else { - status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; + status = car_state.getMadsEnabled() ? car_control.getLongActive() ? STATUS_ENGAGED : STATUS_MADS : STATUS_DISENGAGED; } } @@ -215,6 +217,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", + "carControl", }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9e1c54948b..6618f0267f 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -73,6 +73,7 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + STATUS_MADS, STATUS_WARNING, STATUS_ALERT, } UIStatus; @@ -80,7 +81,8 @@ typedef enum UIStatus { const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), - [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + [STATUS_ENGAGED] = QColor(0x00, 0xc8, 0x00, 0xf1), + [STATUS_MADS] = QColor(0x00, 0xc8, 0xc8, 0xf1), [STATUS_WARNING] = QColor(0xDA, 0x6F, 0x25, 0xf1), [STATUS_ALERT] = QColor(0xC9, 0x22, 0x31, 0xf1), }; diff --git a/system/version.py b/system/version.py index 6031531556..8aec8f7bd7 100644 --- a/system/version.py +++ b/system/version.py @@ -7,8 +7,8 @@ from functools import lru_cache from common.basedir import BASEDIR from system.swaglog import cloudlog -RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3'] -TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging'] +RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'prod-c3'] +TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging', 'test-c3'] training_version: bytes = b"0.2.0" terms_version: bytes = b"2" @@ -90,7 +90,7 @@ def is_comma_remote() -> bool: if origin is None: return False - return origin.startswith('git@github.com:commaai') or origin.startswith('https://github.com/commaai') + return origin.startswith('git@github.com:sunnyhaibin') or origin.startswith('https://github.com/sunnyhaibin') @cache