From a59f46c7b2f2d01d9e941751b54dc0c3e8556aa2 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 8 Aug 2026 21:46:49 -0500 Subject: [PATCH] Leafy Greens --- opendbc_repo/docs/CARS.md | 4 +- opendbc_repo/opendbc/car/disable_ecu.py | 11 +- .../opendbc/car/hyundai/carcontroller.py | 6 +- opendbc_repo/opendbc/car/hyundai/carstate.py | 10 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 58 +++++++ .../opendbc/car/nissan/carcontroller.py | 36 ++++- opendbc_repo/opendbc/car/nissan/carstate.py | 14 ++ opendbc_repo/opendbc/car/nissan/interface.py | 67 +++++++- opendbc_repo/opendbc/car/nissan/nissancan.py | 33 ++++ .../opendbc/car/nissan/tests/__init__.py | 1 + .../opendbc/car/nissan/tests/test_nissan.py | 149 ++++++++++++++++++ .../car/nissan/tests/test_nissancan.py | 38 +++++ opendbc_repo/opendbc/car/nissan/values.py | 19 ++- opendbc_repo/opendbc/car/subaru/carstate.py | 2 +- .../opendbc/car/subaru/tests/test_subaru.py | 8 +- opendbc_repo/opendbc/car/subaru/values.py | 3 +- .../dbc/generator/nissan/_nissan_common.dbc | 2 +- .../dbc/nissan_leaf_2018_generated.dbc | 2 +- .../dbc/nissan_x_trail_2017_generated.dbc | 2 +- opendbc_repo/opendbc/safety/modes/nissan.h | 117 +++++++++++++- opendbc_repo/opendbc/safety/modes/subaru.h | 8 +- opendbc_repo/opendbc/safety/tests/common.py | 3 +- .../opendbc/safety/tests/test_nissan.py | 129 +++++++++++++++ .../opendbc/safety/tests/test_subaru.py | 14 +- selfdrive/car/car_specific.py | 3 + selfdrive/car/card.py | 5 +- selfdrive/controls/lib/latcontrol_torque.py | 4 +- .../controls/lib/latcontrol_vehicle_tunes.py | 26 ++- selfdrive/controls/tests/test_latcontrol.py | 13 +- selfdrive/selfdrived/selfdrived.py | 3 +- .../selfdrived/tests/test_blindspot_alerts.py | 10 +- .../ui/layouts/settings/starpilot/sounds.py | 4 +- starpilot/common/starpilot_variables.py | 15 +- .../tools/device_settings_layout.json | 16 -- tools/StarPilot/generate_galaxy_layout.py | 2 +- 35 files changed, 753 insertions(+), 84 deletions(-) create mode 100644 opendbc_repo/opendbc/car/nissan/tests/__init__.py create mode 100644 opendbc_repo/opendbc/car/nissan/tests/test_nissan.py create mode 100644 opendbc_repo/opendbc/car/nissan/tests/test_nissancan.py diff --git a/opendbc_repo/docs/CARS.md b/opendbc_repo/docs/CARS.md index 47e41bbdf..778a36333 100644 --- a/opendbc_repo/docs/CARS.md +++ b/opendbc_repo/docs/CARS.md @@ -243,7 +243,7 @@ |Mazda|CX-9 2016-20|All|[Dashcam mode](#dashcam)| |Mazda|CX-9 2021-23|All|[Upstream](#upstream)| |Nissan|Altima 2019-20, 2024|ProPILOT Assist|[Upstream](#upstream)| -|Nissan|Leaf 2018-23|ProPILOT Assist|[Upstream](#upstream)| +|Nissan|Leaf 2018-25|ProPILOT Assist|[Upstream](#upstream)| |Nissan|Rogue 2018-20|ProPILOT Assist|[Upstream](#upstream)| |Nissan|X-Trail 2017|ProPILOT Assist|[Upstream](#upstream)| |Peugeot|208 2019-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)| @@ -444,4 +444,4 @@ Toyota, and the GM Global B platform. All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars -may one day be supported, but we have no immediate plans to support FlexRay. \ No newline at end of file +may one day be supported, but we have no immediate plans to support FlexRay. diff --git a/opendbc_repo/opendbc/car/disable_ecu.py b/opendbc_repo/opendbc/car/disable_ecu.py index a3e1f5995..26c4d9580 100644 --- a/opendbc_repo/opendbc/car/disable_ecu.py +++ b/opendbc_repo/opendbc/car/disable_ecu.py @@ -23,9 +23,11 @@ def ecu_log(msg): pass -def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, reset=False): +def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, reset=False, + require_response=False, diag_request=EXT_DIAG_REQUEST, diag_response=EXT_DIAG_RESPONSE): """Silence an ECU by disabling sending and receiving messages using UDS 0x28. The ECU will stay silent as long as openpilot keeps sending Tester Present. + Set require_response for takeovers that must fail closed unless the ECU confirms communication control. This is used to disable the radar in some cars. Openpilot will emulate the radar. WARNING: THIS DISABLES AEB!""" @@ -45,7 +47,7 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r try: # Enter extended diagnostic session ecu_log(f"attempt {i+1}/{retry}: diag session...") - query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE]) + query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [diag_request], [diag_response]) for _, _ in query.get_data(timeout).items(): ecu_log("diag session OK") @@ -62,7 +64,7 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r cc_success = False cc_rejected = False cc_nrc = None - for (rx_addr, _), data in cc_response.items(): + for (_rx_addr, _), data in cc_response.items(): ecu_log(f"CC response: {data.hex() if data else 'empty'}") # Check for positive response (0x68 = 0x28 + 0x40) if len(data) >= 1 and data[0] == 0x68: @@ -93,6 +95,9 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r # ECU explicitly rejected - don't retry, it won't work ecu_log("=== ECU DISABLE REJECTED ===") return False + elif require_response: + ecu_log("CC response required but none received; retrying...") + continue else: # No response - consider it sent (ECU might have stopped responding) ecu_log("=== ECU DISABLE SENT (no response) ===") diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 2f84ec0ed..49d7433db 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -774,7 +774,11 @@ class CarController(CarControllerBase): ) # steering control - preserve_stock_lkas = bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and not self.long_active_ecu + # The first-generation Electrified GV70 expects the synthesized LKAS status + # payload. Forwarding its stock status bits leaves lane-safety state asserted + # while StarPilot is suppressing the stock LFA path. + preserve_stock_lkas = bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and \ + not self.long_active_ecu and self.CP.carFingerprint != CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN angle_lkas_alt = bool(self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT) ccnc_angle_long = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and \ diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index b9b316fdd..eed419217 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -603,13 +603,15 @@ class CarState(CarStateBase): msgs = [] cam_msgs = [] if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS): - # TODO: this can be removed once we add dynamic support to vl_all + # The EV9 can stop publishing this during the non-ECU-disabled startup + # state. Keep decoding it when present without making CAN invalid. msgs += [ - # this message is 50Hz but the ECU frequently stops transmitting for ~0.5s - ("CRUISE_BUTTONS", 1) + ("CRUISE_BUTTONS", 0 if CP.carFingerprint == CAR.KIA_EV9 else 1) ] if CP.flags & HyundaiFlags.CANFD_LKA_STEERING: - msgs.append(("FR_CMR_02_100ms", 10)) + # EV9 camera status can disappear for an extended period when the + # documented ECU startup sequence is skipped. + msgs.append(("FR_CMR_02_100ms", 0 if CP.carFingerprint == CAR.KIA_EV9 else 10)) msgs.append(("FR_CMR_03_50ms", 0)) cam_msgs.append(("LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS", 0)) else: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 201babbbe..65463a3d3 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -132,6 +132,20 @@ class TestHyundaiFingerprint: assert "BLINDSPOTS_REAR_CORNERS" in pt_messages + def test_ev9_startup_status_messages_are_optional(self): + fingerprint = gen_empty_fingerprint() + fingerprint[CanBus(None, fingerprint).CAM][0x110] = 32 + radar_config = get_radar_track_config(CAR.KIA_EV9) + fingerprint[radar_config.bus][radar_config.start_addr] = radar_config.expected_length + car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")] + + CP = CarInterface.get_params(CAR.KIA_EV9, fingerprint, car_fw, False, False, False, None) + parsers = CarState(CP, None).get_can_parsers(CP) + states = parsers[Bus.pt].message_states + + assert states[0x1CF].ignore_alive + assert states[0x1FA].ignore_alive + def test_feature_detection(self): # LKA steering for candidate in (CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_6): @@ -1811,6 +1825,50 @@ class TestHyundaiFingerprint: assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0) assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(8.5) + def test_gv70_electrified_synthesizes_lkas_status_payload(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING) + CP.openpilotLongitudinalControl = False + + controller = CarController(DBC[CP.carFingerprint], CP) + controller.frame = 1 + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS", 0)], can_bus.ACAN) + stock_lkas = { + "CHECKSUM": 1234, + "COUNTER": 42, + "LKA_MODE": 2, + "LKA_AVAILABLE": 0, + "LKA_WARNING": 0, + "LKA_ICON": 1, + "FCA_SYSWARN": 0, + "TORQUE_REQUEST": 17, + "STEER_REQ": 1, + "LFA_BUTTON": 0, + "LKA_ASSIST": 0, + "STEER_MODE": 2, + "NEW_SIGNAL_2": 3, + "HAS_LANE_SAFETY": 1, + "DAMP_FACTOR": 100, + } + cc = SimpleNamespace(enabled=True, latActive=True, + actuators=SimpleNamespace(longControlState=LongCtrlState.off), + leftBlinker=False, rightBlinker=False, + hudControl=SimpleNamespace()) + cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas, + out=SimpleNamespace(steeringAngleDeg=0.0, + gearShifter=structs.CarState.GearShifter.drive)) + + msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False, + cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2) + lkas_msgs = [msg for msg in msgs if msg[0] == 0x50] + assert len(lkas_msgs) == 1 + + parser.update([(1, lkas_msgs)]) + assert parser.can_valid + assert parser.vl["LKAS"]["HAS_LANE_SAFETY"] == 0 + def test_ev9_inactive_angle_steering_lets_safety_forward_stock_lkas(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV9 diff --git a/opendbc_repo/opendbc/car/nissan/carcontroller.py b/opendbc_repo/opendbc/car/nissan/carcontroller.py index 381addc73..493ed41bc 100644 --- a/opendbc_repo/opendbc/car/nissan/carcontroller.py +++ b/opendbc_repo/opendbc/car/nissan/carcontroller.py @@ -1,7 +1,7 @@ import numpy as np from opendbc.can import CANPacker -from opendbc.car import Bus, DT_CTRL, structs +from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs from opendbc.car.common.filter_simple import FirstOrderFilter from opendbc.car.lateral import apply_std_steer_angle_limits from opendbc.car.interfaces import CarControllerBase @@ -9,6 +9,7 @@ from opendbc.car.nissan import nissancan from opendbc.car.nissan.values import CAR, CarControllerParams VisualAlert = structs.CarControl.HUDControl.VisualAlert +LongCtrlState = structs.CarControl.Actuators.LongControlState class CarController(CarControllerBase): @@ -28,6 +29,34 @@ class CarController(CarControllerBase): can_sends = [] + if self.CP.openpilotLongitudinalControl and self.car_fingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): + accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) + stopping = actuators.longControlState == LongCtrlState.stopping + brake_mode = CC.longActive and (accel < CarControllerParams.PROPILOT_ACCEL_MIN or stopping) + + if CC.longActive: + if brake_mode: + raw_accel = CarControllerParams.PROPILOT_BRAKE_RAW + else: + propulsion_accel = float(np.clip(accel, CarControllerParams.PROPILOT_ACCEL_MIN, + CarControllerParams.PROPILOT_ACCEL_MAX)) + raw_accel = round(propulsion_accel * CarControllerParams.PROPILOT_ACCEL_SCALE + + CarControllerParams.PROPILOT_ACCEL_OFFSET) + else: + raw_accel = CarControllerParams.PROPILOT_INACTIVE_RAW + + brake_pressure = round(max(0.0, -accel - 1.0) * CarControllerParams.BRAKE_GAIN) if brake_mode else 0 + if stopping and CS.out.vEgo < self.CP.vEgoStopping: + brake_pressure = CarControllerParams.BRAKE_MAX + brake_pressure = int(np.clip(brake_pressure, 0, CarControllerParams.BRAKE_MAX)) + + can_sends.append(nissancan.create_accel_command(raw_accel, self.frame, CC.longActive)) + can_sends.append(nissancan.create_brake_command(brake_pressure, self.frame, + CC.longActive and brake_pressure > 0, brake_mode)) + + if self.frame % 100 == 0: + can_sends.append(make_tester_present_msg(0x707, 1, suppress_response=True)) + ### STEER ### steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 @@ -64,7 +93,8 @@ class CarController(CarControllerBase): # We now cancel by making propilot think the seatbelt is unlatched, # this generates a beep and a warning message every time you disengage if self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC) and self.frame % 2 == 0: - can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) + can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, + pcm_cancel_cmd and not self.CP.openpilotLongitudinalControl)) can_sends.append(nissancan.create_steering_control( self.packer, self.apply_angle_last, self.frame, CC.latActive, lkas_max_torque)) @@ -82,6 +112,8 @@ class CarController(CarControllerBase): new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_last + if self.CP.openpilotLongitudinalControl: + new_actuators.accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/nissan/carstate.py b/opendbc_repo/opendbc/car/nissan/carstate.py index 57d8fc2cd..4f297986c 100644 --- a/opendbc_repo/opendbc/car/nissan/carstate.py +++ b/opendbc_repo/opendbc/car/nissan/carstate.py @@ -26,6 +26,9 @@ class CarState(CarStateBase): self.distance_button = 0 self.lkas_button = 0 + self.set_button = 0 + self.res_button = 0 + self.cancel_button = 0 def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -131,6 +134,17 @@ class CarState(CarStateBase): buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + if self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): + prev_set_button = self.set_button + prev_res_button = self.res_button + prev_cancel_button = self.cancel_button + self.set_button = int(cp.vl["CRUISE_THROTTLE"]["SET_BUTTON"]) + self.res_button = int(cp.vl["CRUISE_THROTTLE"]["RES_BUTTON"]) + self.cancel_button = int(cp.vl["CRUISE_THROTTLE"]["CANCEL_BUTTON"]) + buttonEvents += create_button_events(self.set_button, prev_set_button, {1: ButtonType.decelCruise}) + buttonEvents += create_button_events(self.res_button, prev_res_button, {1: ButtonType.accelCruise}) + buttonEvents += create_button_events(self.cancel_button, prev_cancel_button, {1: ButtonType.cancel}) + fp_ret = custom.StarPilotCarState.new_message() self.prev_lkas_button = self.lkas_button diff --git a/opendbc_repo/opendbc/car/nissan/interface.py b/opendbc_repo/opendbc/car/nissan/interface.py index bac7b3f1a..688449505 100644 --- a/opendbc_repo/opendbc/car/nissan/interface.py +++ b/opendbc_repo/opendbc/car/nissan/interface.py @@ -1,14 +1,27 @@ -from opendbc.car import get_safety_config, structs +from opendbc.car import get_safety_config, structs, uds +from opendbc.car.disable_ecu import disable_ecu, ecu_log from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.nissan.carcontroller import CarController from opendbc.car.nissan.carstate import CarState -from opendbc.car.nissan.values import CAR, NissanSafetyFlags +from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags, \ + NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_DIAGNOSTIC_RESPONSE_KWP + + +LEAF_LONGITUDINAL_CARS = (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC) +LEAF_ADAS_ECU_ADDR = 0x707 +LEAF_ADAS_ECU_BUS = 1 class CarInterface(CarInterfaceBase): CarState = CarState CarController = CarController + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + if CP.carFingerprint in LEAF_LONGITUDINAL_CARS and CP.openpilotLongitudinalControl: + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + return CarInterfaceBase.get_pid_accel_limits(CP, current_speed, cruise_speed) + @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: ret.brand = "nissan" @@ -22,8 +35,58 @@ class CarInterface(CarInterfaceBase): ret.steerControlType = structs.CarParams.SteerControlType.angle ret.radarUnavailable = True + ret.alphaLongitudinalAvailable = candidate in LEAF_LONGITUDINAL_CARS + ret.openpilotLongitudinalControl = alpha_long and ret.alphaLongitudinalAvailable + ret.pcmCruise = not ret.openpilotLongitudinalControl + + if ret.openpilotLongitudinalControl: + ret.safetyConfigs[0].safetyParam |= NissanSafetyFlags.LONG_CONTROL.value + ret.autoResumeSng = True + ret.stopAccel = -2.0 + ret.vEgoStopping = 0.5 + ret.vEgoStarting = 0.5 + ret.stoppingDecelRate = 0.8 + if candidate == CAR.NISSAN_ALTIMA: # Altima has EPS on C-CAN unlike the others that have it on V-CAN ret.safetyConfigs[0].safetyParam |= NissanSafetyFlags.ALT_EPS_BUS.value return ret + + @staticmethod + def init(CP, can_recv, can_send): + if not (CP.openpilotLongitudinalControl and CP.carFingerprint in LEAF_LONGITUDINAL_CARS): + return + + from openpilot.common.params import Params + params = Params() + communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, + uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, + uds.MESSAGE_TYPE.NORMAL]) + ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR, + com_cont_req=communication_control, require_response=True) + if not ecu_disabled: + # Nissan firmware queries use the KWP-style default session. Try it after + # standard UDS extended-session control, but still require a positive 0x68 response. + ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR, + com_cont_req=communication_control, require_response=True, + diag_request=NISSAN_DIAGNOSTIC_REQUEST_KWP, diag_response=NISSAN_DIAGNOSTIC_RESPONSE_KWP) + params.put_bool("EcuDisableFailed", not ecu_disabled) + if ecu_disabled: + ecu_log("Nissan Leaf ADAS TX disabled; experimental longitudinal control enabled") + else: + CP.safetyConfigs[-1].safetyParam &= ~NissanSafetyFlags.LONG_CONTROL.value + CP.openpilotLongitudinalControl = False + CP.pcmCruise = True + ecu_log("Nissan Leaf ADAS TX disable failed; falling back to stock longitudinal control") + + @staticmethod + def deinit(CP, can_recv, can_send): + if not (CP.openpilotLongitudinalControl and CP.carFingerprint in LEAF_LONGITUDINAL_CARS): + return + + communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, + 0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX, + uds.MESSAGE_TYPE.NORMAL]) + disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR, + com_cont_req=communication_control) diff --git a/opendbc_repo/opendbc/car/nissan/nissancan.py b/opendbc_repo/opendbc/car/nissan/nissancan.py index 0c95ad185..e49b176a8 100644 --- a/opendbc_repo/opendbc/car/nissan/nissancan.py +++ b/opendbc_repo/opendbc/car/nissan/nissancan.py @@ -1,10 +1,43 @@ import crcmod +from opendbc.car.can_definitions import CanData from opendbc.car.nissan.values import CAR # TODO: add this checksum to the CANPacker nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) +def create_accel_command(raw_command, frame, active): + """Build the Leaf ADAS propulsion/regen request (0x2B0).""" + raw_command = int(raw_command) + raw12, fraction = divmod(raw_command, 4) + inverse = (~raw12) & 0xFFF + + dat = bytes([ + ((inverse & 0xF) << 4) | ((inverse >> 4) & 0xF), + (raw12 >> 4) & 0xFF, + (((inverse >> 8) & 0xF) << 4) | (raw12 & 0xF), + 0xAC, # normal ADAS ownership state; stock briefly uses 0x6C while handing control back + 0x5B if active else 0x1B, + 0x00, + ((frame & 0xF) << 4) | 0xE, + (fraction << 2) | ((~fraction) & 0x3), + ]) + return CanData(0x2B0, dat, 1) + + +def create_brake_command(pressure, frame, active, brake_mode): + """Build the Leaf ADAS friction-brake request (0x1C3), with pressure in 0.5-count units.""" + pressure = int(pressure) + dat = bytearray(8) + dat[0] = (pressure >> 4) & 0x3F + dat[1] = (pressure & 0xF) << 4 + dat[4] = 0x64 + dat[5] = (0x80 if active else 0) | (0x04 if brake_mode else 0) | (frame & 0x3) + dat[6] = 0xFF + dat[7] = (0x01 + 0xC3 + sum(dat[:7])) & 0xFF + return CanData(0x1C3, bytes(dat), 1) + + def create_steering_control(packer, apply_torque, frame, steer_on, lkas_max_torque): values = { "COUNTER": frame % 0x10, diff --git a/opendbc_repo/opendbc/car/nissan/tests/__init__.py b/opendbc_repo/opendbc/car/nissan/tests/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/opendbc_repo/opendbc/car/nissan/tests/__init__.py @@ -0,0 +1 @@ + diff --git a/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py b/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py new file mode 100644 index 000000000..56cf442aa --- /dev/null +++ b/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py @@ -0,0 +1,149 @@ +from types import SimpleNamespace + +import pytest + +from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs, uds +from opendbc.car.nissan.carstate import CarState +from opendbc.car.nissan.interface import CarInterface +from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags + + +TEST_TOGGLES = SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False, trailer_load_kg=0) + + +def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.CarControl.Actuators.LongControlState.pid): + CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), [], alpha_long, False, False, TEST_TOGGLES) + FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), [], CP, TEST_TOGGLES) + CI = CarInterface(CP, FPCP) + CI.update([], TEST_TOGGLES) + + CC = structs.CarControl() + CC.enabled = True + CC.longActive = long_active + CC.actuators.accel = accel + CC.actuators.longControlState = long_state + _, can_sends = CI.apply(CC.as_reader(), 0, TEST_TOGGLES) + return {msg[0]: msg for msg in can_sends} + + +@pytest.mark.parametrize("candidate", [CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC]) +def test_leaf_alpha_long_params(candidate): + stock = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None) + alpha_long = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None) + + assert stock.alphaLongitudinalAvailable + assert not stock.openpilotLongitudinalControl + assert stock.pcmCruise + assert not (stock.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) + + assert alpha_long.alphaLongitudinalAvailable + assert alpha_long.openpilotLongitudinalControl + assert not alpha_long.pcmCruise + assert alpha_long.autoResumeSng + assert alpha_long.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL + assert CarInterface.get_pid_accel_limits(alpha_long, 0, 0) == (CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + +def test_non_leaf_does_not_offer_alpha_long(): + CP = CarInterface.get_params(CAR.NISSAN_ROGUE, gen_empty_fingerprint(), [], True, False, False, None) + + assert not CP.alphaLongitudinalAvailable + assert not CP.openpilotLongitudinalControl + assert CP.pcmCruise + assert not (CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) + + +def test_stock_controller_does_not_send_longitudinal_messages(): + can_sends = run_controller(False) + + assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys()) + + +def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(): + can_sends = run_controller(True) + + assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03" + assert can_sends[0x1C3][1].hex() == "000000006400ff27" + assert can_sends[0x707][1].hex() == "023e800000000000" + assert all(can_sends[addr][2] == 1 for addr in (0x2B0, 0x1C3, 0x707)) + + +def test_alpha_long_controller_clamps_to_panda_accel_limit(): + can_sends = run_controller(True, accel=5.0) + + assert can_sends[0x2B0][1].hex() == "007f8fac5b000e0c" + + +def test_alpha_long_controller_blends_friction_brake_below_regen_limit(): + can_sends = run_controller(True, accel=-2.0) + + assert can_sends[0x2B0][1].hex() == "a827d5ac5b000e09" + brake = can_sends[0x1C3][1] + assert ((brake[0] & 0x3F) << 4) | (brake[1] >> 4) == 264 + assert brake[5] & 0x84 == 0x84 + + +def test_alpha_long_controller_sends_inactive_commands_when_disengaged(): + can_sends = run_controller(True, accel=1.0, long_active=False) + + assert can_sends[0x2B0][1].hex() == "dc53a2ac1b000e03" + assert can_sends[0x1C3][1].hex() == "000000006400ff27" + + +@pytest.mark.parametrize(("signal", "button_type"), [("SET_BUTTON", ButtonType.decelCruise), + ("RES_BUTTON", ButtonType.accelCruise)]) +def test_leaf_set_resume_release_enables_alpha_long(signal, button_type): + CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), [], True, False, False, TEST_TOGGLES) + FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), [], CP, TEST_TOGGLES) + CS = CarState(CP, FPCP) + parsers = CS.get_can_parsers(CP) + + parsers[Bus.pt].vl["CRUISE_THROTTLE"][signal] = 1 + pressed, _ = CS.update(parsers, TEST_TOGGLES) + parsers[Bus.pt].vl["CRUISE_THROTTLE"][signal] = 0 + released, _ = CS.update(parsers, TEST_TOGGLES) + + assert any(event.type == button_type and event.pressed for event in pressed.buttonEvents) + assert any(event.type == button_type and not event.pressed for event in released.buttonEvents) + assert CS.update_button_enable(released.buttonEvents) + + +@pytest.mark.parametrize("ecu_disabled", [False, True]) +def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled): + CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), [], True, False, False, None) + calls = [] + + def fake_disable_ecu(*args, **kwargs): + calls.append(kwargs) + return ecu_disabled + + monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", fake_disable_ecu) + monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None) + CarInterface.init(CP, None, None) + + assert len(calls) == (1 if ecu_disabled else 2) + assert calls[0]["addr"] == 0x707 + assert calls[0]["bus"] == 1 + assert calls[0]["require_response"] is True + assert calls[0]["com_cont_req"] == bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, + uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, + uds.MESSAGE_TYPE.NORMAL]) + if not ecu_disabled: + assert calls[1]["diag_request"] == b"\x10\x81" + assert calls[1]["diag_response"] == b"\x50\x81" + assert CP.openpilotLongitudinalControl is ecu_disabled + assert CP.pcmCruise is not ecu_disabled + assert bool(CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) is ecu_disabled + + +def test_leaf_kwp_session_can_confirm_ecu_disable(monkeypatch): + CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), [], True, False, False, None) + results = iter((False, True)) + + monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: next(results)) + monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None) + CarInterface.init(CP, None, None) + + assert CP.openpilotLongitudinalControl + assert not CP.pcmCruise + assert CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL diff --git a/opendbc_repo/opendbc/car/nissan/tests/test_nissancan.py b/opendbc_repo/opendbc/car/nissan/tests/test_nissancan.py new file mode 100644 index 000000000..cd19e2cc2 --- /dev/null +++ b/opendbc_repo/opendbc/car/nissan/tests/test_nissancan.py @@ -0,0 +1,38 @@ +import pytest + +from opendbc.car.nissan import nissancan + + +@pytest.mark.parametrize( + ("raw_command", "frame", "active", "expected"), + [ + (5320, 0, False, "dc53a2ac1b000e03"), + (6144, 0, True, "ff6090ac5b000e03"), + (8191, 15, True, "007f8fac5b00fe0c"), + (4096, 0, True, "ff40b0ac5b000e03"), + (2518, 0, True, "a827d5ac5b000e09"), + ], +) +def test_leaf_accel_command_vectors(raw_command, frame, active, expected): + addr, dat, bus = nissancan.create_accel_command(raw_command, frame, active) + + assert addr == 0x2B0 + assert bus == 1 + assert dat.hex() == expected + + +@pytest.mark.parametrize( + ("pressure", "frame", "active", "brake_mode", "expected"), + [ + (0, 0, False, False, "000000006400ff27"), + (8, 3, True, False, "008000006483ff2a"), + (142, 2, True, True, "08e000006486ff95"), + ], +) +def test_leaf_brake_command_vectors(pressure, frame, active, brake_mode, expected): + addr, dat, bus = nissancan.create_brake_command(pressure, frame, active, brake_mode) + + assert addr == 0x1C3 + assert bus == 1 + assert dat.hex() == expected + assert dat[7] == (0x01 + 0xC3 + sum(dat[:7])) & 0xFF diff --git a/opendbc_repo/opendbc/car/nissan/values.py b/opendbc_repo/opendbc/car/nissan/values.py index 4f4a596c1..cb46c415d 100644 --- a/opendbc_repo/opendbc/car/nissan/values.py +++ b/opendbc_repo/opendbc/car/nissan/values.py @@ -22,12 +22,27 @@ class CarControllerParams: LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 + # 2025 Leaf ProPILOT longitudinal command limits. The propulsion request has + # 2048 counts per m/s^2 around a 6144-count zero point. Friction braking is + # blended in below -1 m/s^2 using the stock pressure range seen in logs. + ACCEL_MIN = -3.5 + ACCEL_MAX = 2047 / 2048 + PROPILOT_ACCEL_MIN = -1.0 + PROPILOT_ACCEL_MAX = ACCEL_MAX + PROPILOT_ACCEL_OFFSET = 6144 + PROPILOT_ACCEL_SCALE = 2048 + PROPILOT_INACTIVE_RAW = 5320 + PROPILOT_BRAKE_RAW = 2518 + BRAKE_MAX = 659 # 0.5 pressure-count units; stock peak is 329.5 + BRAKE_GAIN = 264.0 + def __init__(self, CP): pass class NissanSafetyFlags(IntFlag): ALT_EPS_BUS = 1 + LONG_CONTROL = 2 class Footnote(Enum): @@ -60,13 +75,13 @@ class CAR(Platforms): NissanCarSpecs(mass=1610, wheelbase=2.705) ) NISSAN_LEAF = NissanPlatformConfig( - [NissanCarDocs("Nissan Leaf 2018-23", video="https://youtu.be/vaMbtAh_0cY")], + [NissanCarDocs("Nissan Leaf 2018-25", video="https://youtu.be/vaMbtAh_0cY")], NissanCarSpecs(mass=1610, wheelbase=2.705), {Bus.pt: 'nissan_leaf_2018_generated'}, ) # Leaf with ADAS ECU found behind instrument cluster instead of glovebox # Currently the only known difference between them is the inverted seatbelt signal. - NISSAN_LEAF_IC = NISSAN_LEAF.override(car_docs=[NissanCarDocs("Nissan Leaf Instrument Cluster 2018-23", video=NISSAN_LEAF.car_docs[0].video)]) + NISSAN_LEAF_IC = NISSAN_LEAF.override(car_docs=[NissanCarDocs("Nissan Leaf Instrument Cluster 2018-25", video=NISSAN_LEAF.car_docs[0].video)]) NISSAN_ROGUE = NissanPlatformConfig( [NissanCarDocs("Nissan Rogue 2018-20")], NissanCarSpecs(mass=1610, wheelbase=2.705) diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index 928eb7dcb..899cc7dfc 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -21,7 +21,7 @@ class CarState(CarStateBase): cp_cam = can_parsers[Bus.cam] cp_alt = can_parsers[Bus.alt] cp_main = can_parsers[Bus.main] if self.CP.flags & SubaruFlags.D_PLATFORM else cp - cp_angle = cp_main if self.CP.flags & SubaruFlags.D_PLATFORM else cp + cp_angle = cp_cam if self.CP.flags & SubaruFlags.D_PLATFORM else cp ret = structs.CarState() throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"] diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 94c7ea384..05a0b7b18 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -136,12 +136,12 @@ def test_outback_2023_uses_d_platform_bus_layout(): assert CP.flags & SubaruFlags.D_PLATFORM assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM assert CanBus.main_for_cp(CP) == CanBus.alt - assert CanBus.angle_for_cp(CP) == CanBus.main + assert CanBus.angle_for_cp(CP) == CanBus.camera assert parsers[Bus.pt].bus == CanBus.alt assert parsers[Bus.cam].bus == CanBus.camera assert parsers[Bus.alt].bus == CanBus.alt assert parsers[Bus.main].bus == CanBus.main - assert controller.angle_bus == CanBus.main + assert controller.angle_bus == CanBus.camera assert controller.status_bus == CanBus.camera @@ -153,12 +153,12 @@ def test_legacy_2025_uses_d_platform_bus_layout(): assert CP.flags & SubaruFlags.D_PLATFORM assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM assert CanBus.main_for_cp(CP) == CanBus.alt - assert CanBus.angle_for_cp(CP) == CanBus.main + assert CanBus.angle_for_cp(CP) == CanBus.camera assert parsers[Bus.pt].bus == CanBus.alt assert parsers[Bus.cam].bus == CanBus.camera assert parsers[Bus.alt].bus == CanBus.alt assert parsers[Bus.main].bus == CanBus.main - assert controller.angle_bus == CanBus.main + assert controller.angle_bus == CanBus.camera assert controller.status_bus == CanBus.camera diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index 90b40eea5..48786ab7e 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -113,7 +113,8 @@ class CanBus: @staticmethod def angle_for_cp(CP): - return CanBus.main + # D-platform angle LKAS is exchanged with the camera ECU on the camera bus. + return CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main class Footnote(Enum): diff --git a/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc b/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc index 46a65a7b7..62e8f10f2 100644 --- a/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc +++ b/opendbc_repo/opendbc/dbc/generator/nissan/_nissan_common.dbc @@ -59,7 +59,7 @@ BO_ 689 PROPILOT_HUD: 8 XXX SG_ unknown59 : 59|4@0+ (1,0) [0|15] "" XXX BO_ 451 PROPILOT_BRAKE: 8 XXX - SG_ BRAKE_PRESSURE : 5|9@0+ (1,0) [0|511] "" XXX + SG_ BRAKE_PRESSURE : 5|10@0+ (0.5,0) [0|511.5] "" XXX SG_ BRAKE_ACTIVE : 47|1@0+ (1,0) [0|1] "" XXX BO_ 783 CRUISE_STATE: 3 XXX diff --git a/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc b/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc index c714d2f24..e4dcabd9b 100644 --- a/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc +++ b/opendbc_repo/opendbc/dbc/nissan_leaf_2018_generated.dbc @@ -63,7 +63,7 @@ BO_ 689 PROPILOT_HUD: 8 XXX SG_ unknown59 : 59|4@0+ (1,0) [0|15] "" XXX BO_ 451 PROPILOT_BRAKE: 8 XXX - SG_ BRAKE_PRESSURE : 5|9@0+ (1,0) [0|511] "" XXX + SG_ BRAKE_PRESSURE : 5|10@0+ (0.5,0) [0|511.5] "" XXX SG_ BRAKE_ACTIVE : 47|1@0+ (1,0) [0|1] "" XXX BO_ 783 CRUISE_STATE: 3 XXX diff --git a/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc b/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc index f64dfc266..fa35689e5 100644 --- a/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc +++ b/opendbc_repo/opendbc/dbc/nissan_x_trail_2017_generated.dbc @@ -63,7 +63,7 @@ BO_ 689 PROPILOT_HUD: 8 XXX SG_ unknown59 : 59|4@0+ (1,0) [0|15] "" XXX BO_ 451 PROPILOT_BRAKE: 8 XXX - SG_ BRAKE_PRESSURE : 5|9@0+ (1,0) [0|511] "" XXX + SG_ BRAKE_PRESSURE : 5|10@0+ (0.5,0) [0|511.5] "" XXX SG_ BRAKE_ACTIVE : 47|1@0+ (1,0) [0|1] "" XXX BO_ 783 CRUISE_STATE: 3 XXX diff --git a/opendbc_repo/opendbc/safety/modes/nissan.h b/opendbc_repo/opendbc/safety/modes/nissan.h index 3f3aed63f..c73a94404 100644 --- a/opendbc_repo/opendbc/safety/modes/nissan.h +++ b/opendbc_repo/opendbc/safety/modes/nissan.h @@ -3,6 +3,9 @@ #include "opendbc/safety/declarations.h" static bool nissan_alt_eps = false; +static bool nissan_longitudinal = false; +static bool nissan_set_button_prev = false; +static bool nissan_res_button_prev = false; static void nissan_rx_all_hook(const CANPacket_t *msg) { if ((msg->addr == 0x1B6U) && (msg->bus == (nissan_alt_eps ? 2U : 1U))) { @@ -51,14 +54,33 @@ static void nissan_rx_hook(const CANPacket_t *msg) { } } - // Handle cruise enabled - if ((msg->addr == 0x30fU) && (msg->bus == (nissan_alt_eps ? 1U : 2U))) { + // Handle stock cruise enabled + if (!nissan_longitudinal && (msg->addr == 0x30fU) && (msg->bus == (nissan_alt_eps ? 1U : 2U))) { bool cruise_engaged = (msg->data[0] >> 3) & 1U; pcm_cruise_check(cruise_engaged); } if ((msg->addr == 0x239U) && (msg->bus == 0U)) { acc_main_on = GET_BIT(msg, 17U); + + if (nissan_longitudinal) { + bool set_button = GET_BIT(msg, 27U); + bool res_button = GET_BIT(msg, 28U); + bool cancel_button = GET_BIT(msg, 25U); + + // Match the interface: SET and RES enable on release, while CANCEL and + // switching cruise main off disengage immediately. + if (acc_main_on && ((nissan_set_button_prev && !set_button) || + (nissan_res_button_prev && !res_button))) { + controls_allowed = true; + } + if (cancel_button || !acc_main_on) { + controls_allowed = false; + } + + nissan_set_button_prev = set_button; + nissan_res_button_prev = res_button; + } } } @@ -80,6 +102,13 @@ static bool nissan_tx_hook(const CANPacket_t *msg) { bool tx = true; bool violation = false; + const LongitudinalLimits NISSAN_LONG_LIMITS = { + .max_accel = 2047, // just under +1 m/s^2, 1/2048 m/s^2 + .min_accel = -2048, // -1 m/s^2; stronger braking is checked in 0x1C3 + .inactive_accel = 0, + .max_brake = 659, // 0.5 pressure-count units + }; + // steer cmd checks if (msg->addr == 0x169U) { int desired_angle = ((msg->data[0] << 10) | (msg->data[1] << 2) | ((msg->data[2] >> 6) & 0x3U)); @@ -99,6 +128,58 @@ static bool nissan_tx_hook(const CANPacket_t *msg) { violation |= ((msg->data[1] & 0x3dU) > 0U); } + if (nissan_longitudinal && (msg->addr == 0x2b0U) && (msg->bus == 1U)) { + int raw = (msg->data[1] << 4) | (msg->data[2] & 0xFU); + int inverse = ((msg->data[2] >> 4) << 8) | ((msg->data[0] & 0xFU) << 4) | (msg->data[0] >> 4); + int fraction = (msg->data[7] >> 2) & 0x3U; + int raw_command = (raw * 4) + fraction; + bool active = (msg->data[4] & 0x40U) != 0U; + + violation |= inverse != ((~raw) & 0xFFF); + violation |= msg->data[7] != (uint8_t)((fraction << 2) | ((~fraction) & 0x3)); + violation |= (msg->data[3] != 0xACU) || ((msg->data[4] & 0xBFU) != 0x1BU) || + (msg->data[5] != 0U) || ((msg->data[6] & 0xFU) != 0xEU); + + int desired_accel = 0; + if (active) { + if (raw_command == 2518) { + desired_accel = NISSAN_LONG_LIMITS.min_accel; + } else { + violation |= (raw_command < 4096) || (raw_command > 8191); + desired_accel = raw_command - 6144; + } + } else { + violation |= raw_command != 5320; + } + violation |= active && !get_longitudinal_allowed(); + violation |= longitudinal_accel_checks(desired_accel, NISSAN_LONG_LIMITS); + } + + if (nissan_longitudinal && (msg->addr == 0x1c3U) && (msg->bus == 1U)) { + int brake_pressure = ((msg->data[0] & 0x3FU) << 4) | (msg->data[1] >> 4); + bool brake_active = (msg->data[5] & 0x80U) != 0U; + bool brake_mode = (msg->data[5] & 0x4U) != 0U; + uint8_t checksum = (uint8_t)(0x1U + 0xC3U + msg->data[0] + msg->data[1] + msg->data[2] + msg->data[3] + + msg->data[4] + msg->data[5] + msg->data[6]); + + violation |= ((msg->data[0] & 0xC0U) != 0U) || ((msg->data[1] & 0xFU) != 0U); + violation |= (msg->data[2] != 0U) || (msg->data[3] != 0U) || (msg->data[4] != 0x64U) || + ((msg->data[5] & 0x78U) != 0U) || (msg->data[6] != 0xFFU) || (msg->data[7] != checksum); + violation |= brake_active != (brake_pressure > 0); + violation |= brake_mode && !brake_active; + violation |= longitudinal_brake_checks(brake_pressure, NISSAN_LONG_LIMITS); + if (!get_longitudinal_allowed()) { + violation |= brake_active || brake_mode; + } + } + + if (nissan_longitudinal && (msg->addr == 0x707U) && (msg->bus == 1U)) { + violation |= (msg->data[0] != 0x02U) || (msg->data[1] != 0x3EU) || (msg->data[2] != 0x80U); + for (int i = 3; i < 8; i++) { + violation |= msg->data[i] != 0U; + } + } + if (violation) { tx = false; } @@ -117,6 +198,18 @@ static safety_config nissan_init(uint16_t param) { {0x280, 2, 8, .check_relay = true} // CANCEL_MSG (Leaf) }; + static const CanMsg NISSAN_LONG_TX_MSGS[] = { + {0x169, 0, 8, .check_relay = true}, // LKAS + {0x2b1, 0, 8, .check_relay = true}, // PROPILOT_HUD + {0x4cc, 0, 8, .check_relay = true}, // PROPILOT_HUD_INFO_MSG + {0x20b, 2, 6, .check_relay = false}, // CRUISE_THROTTLE (X-Trail) + {0x20b, 1, 6, .check_relay = false}, // CRUISE_THROTTLE (Altima) + {0x280, 2, 8, .check_relay = true}, // CANCEL_MSG (Leaf) + {0x2b0, 1, 8, .check_relay = true}, // Leaf propulsion/regen request + {0x1c3, 1, 8, .check_relay = true}, // Leaf friction-brake request + {0x707, 1, 8, .check_relay = false}, // Leaf ADAS ECU tester present + }; + // Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model. static RxCheck nissan_rx_checks[] = { {.msg = {{0x2, 0, 5, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, @@ -133,11 +226,29 @@ static safety_config nissan_init(uint16_t param) { {0x1cc, 0, 4, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, // DOORS_LIGHTS / BRAKE }; + // The disabled Leaf ADAS ECU no longer publishes CRUISE_STATE. Longitudinal + // mode enables from physical SET/RES buttons in CRUISE_THROTTLE instead. + static RxCheck nissan_long_rx_checks[] = { + {.msg = {{0x2, 0, 5, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{0x285, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{0x239, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + }; + // EPS Location. false = V-CAN, true = C-CAN const uint16_t NISSAN_PARAM_ALT_EPS_BUS = 1; + const uint16_t NISSAN_PARAM_LONGITUDINAL = 2; nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS); - return BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS); + nissan_longitudinal = false; + #ifdef ALLOW_DEBUG + nissan_longitudinal = GET_FLAG(param, NISSAN_PARAM_LONGITUDINAL); + #endif + nissan_set_button_prev = false; + nissan_res_button_prev = false; + + // cppcheck-suppress knownConditionTrueFalse + return nissan_longitudinal ? BUILD_SAFETY_CFG(nissan_long_rx_checks, NISSAN_LONG_TX_MSGS) : + BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS); } const safety_hooks nissan_hooks = { diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index ae7ca6cb9..94f69cedf 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -56,7 +56,7 @@ {MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \ #define SUBARU_D_PLATFORM_ANGLE_TX_MSGS() \ - {MSG_SUBARU_ES_LKAS_ANGLE, SUBARU_MAIN_BUS, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_LKAS_ANGLE, SUBARU_CAM_BUS, 8, .check_relay = true}, \ {MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, .check_relay = true}, \ {MSG_SUBARU_ES_LKAS_State, SUBARU_CAM_BUS, 8, .check_relay = true}, \ {MSG_SUBARU_ES_Infotainment, SUBARU_CAM_BUS, 8, .check_relay = true}, \ @@ -93,8 +93,8 @@ #define SUBARU_D_PLATFORM_ANGLE_RX_CHECKS() \ {.msg = {{MSG_SUBARU_Throttle, SUBARU_ALT_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ - {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ - {.msg = {{MSG_SUBARU_Steering_2, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_CAM_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_2, SUBARU_CAM_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_Brake_Status, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_ES_Brake, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ @@ -127,7 +127,7 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) { static void subaru_rx_hook(const CANPacket_t *msg) { const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; const unsigned int status_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_CAM_BUS; - const unsigned int steering_bus = SUBARU_MAIN_BUS; + const unsigned int steering_bus = subaru_d_platform ? SUBARU_CAM_BUS : SUBARU_MAIN_BUS; const unsigned int main_bus = subaru_d_platform ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == steering_bus)) { diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 75f278e20..a1c215098 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -1048,7 +1048,8 @@ class SafetyTest(SafetyTestBase): msg = make_msg(bus, addr) self.safety.set_controls_allowed(1) # TODO: this should be blocked - if current_test in ["TestNissanSafety", "TestNissanSafetyAltEpsBus", "TestNissanLeafSafety"] and [addr, bus] in self.TX_MSGS: + nissan_tests = ["TestNissanSafety", "TestNissanSafetyAltEpsBus", "TestNissanLeafSafety", "TestNissanLeafLongSafety"] + if current_test in nissan_tests and [addr, bus] in self.TX_MSGS: continue self.assertFalse(self._tx(msg), f"transmit of {addr=:#x} {bus=} from {test_name} during {current_test} was allowed") diff --git a/opendbc_repo/opendbc/safety/tests/test_nissan.py b/opendbc_repo/opendbc/safety/tests/test_nissan.py index 0acb7c648..1251dcad9 100755 --- a/opendbc_repo/opendbc/safety/tests/test_nissan.py +++ b/opendbc_repo/opendbc/safety/tests/test_nissan.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import unittest +from opendbc.car import make_tester_present_msg +from opendbc.car.nissan import nissancan from opendbc.car.nissan.values import NissanSafetyFlags from opendbc.car.structs import CarParams from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -141,5 +143,132 @@ class TestNissanLeafSafety(TestNissanSafety): return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values) +class TestNissanLeafLongSafety(TestNissanLeafSafety): + + TX_MSGS = [*TestNissanLeafSafety.TX_MSGS, [0x2B0, 1], [0x1C3, 1], [0x707, 1]] + RELAY_MALFUNCTION_ADDRS = {0: (0x169, 0x2B1, 0x4CC), 1: (0x2B0, 0x1C3), 2: (0x280,)} + FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2B1, 0x4CC]} + + def setUp(self): + self.packer = CANPackerSafety("nissan_leaf_2018_generated") + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.nissan, NissanSafetyFlags.LONG_CONTROL) + self.safety.init_tests() + + @staticmethod + def _make_msg(can_data): + return common.make_msg(can_data.src, can_data.address, len(can_data.dat), can_data.dat) + + def _accel_msg(self, raw_command, active=True): + return self._make_msg(nissancan.create_accel_command(raw_command, 0, active)) + + def _brake_msg(self, pressure, active=None, brake_mode=True): + if active is None: + active = pressure > 0 + return self._make_msg(nissancan.create_brake_command(pressure, 0, active, brake_mode)) + + def _button_msg(self, main=True, set_button=False, res_button=False, cancel_button=False): + values = { + "CRUISE_AVAILABLE": main, + "SET_BUTTON": set_button, + "RES_BUTTON": res_button, + "CANCEL_BUTTON": cancel_button, + "NO_BUTTON_PRESSED": not any((set_button, res_button, cancel_button)), + } + return self.packer.make_can_msg_safety("CRUISE_THROTTLE", 0, values) + + def _user_brake_msg(self, brake): + values = {"USER_BRAKE_PRESSED": brake, "CRUISE_AVAILABLE": 1, "NO_BUTTON_PRESSED": 1} + return self.packer.make_can_msg_safety("CRUISE_THROTTLE", 0, values) + + def _user_gas_msg(self, gas): + values = {"GAS_PEDAL": gas, "CRUISE_AVAILABLE": 1, "NO_BUTTON_PRESSED": 1} + return self.packer.make_can_msg_safety("CRUISE_THROTTLE", 0, values) + + # Longitudinal mode uses SET/RES button edges, not CRUISE_STATE. + def test_enable_control_allowed_from_cruise(self): + pass + + def test_disable_control_allowed_from_cruise(self): + pass + + def test_cruise_engaged_prev(self): + pass + + def test_aol_remains_allowed_after_cruise_cancel(self): + pass + + def test_set_and_resume_enable_on_release(self): + for button in ("set_button", "res_button"): + with self.subTest(button=button): + self._reset_safety_hooks() + self.safety.init_tests() + self._rx(self._button_msg(main=True, **{button: True})) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._button_msg(main=True)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_cancel_and_main_off_disable(self): + for msg in (self._button_msg(cancel_button=True), self._button_msg(main=False)): + self.safety.set_controls_allowed(True) + self._rx(msg) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_accel_command_limits_and_inactive(self): + self.safety.set_controls_allowed(False) + self.assertTrue(self._tx(self._accel_msg(5320, active=False))) + for raw_command in (4096, 6144, 8191, 2518): + self.assertFalse(self._tx(self._accel_msg(raw_command))) + + self.safety.set_controls_allowed(True) + for raw_command in (4096, 6144, 8191, 2518): + self.assertTrue(self._tx(self._accel_msg(raw_command))) + for raw_command in (0, 2517, 2519, 4095, 8192, 0x3FFF): + self.assertFalse(self._tx(self._accel_msg(raw_command))) + + def test_accel_redundancy_and_constants(self): + self.safety.set_controls_allowed(True) + valid = self._accel_msg(6144) + for index in range(8): + dat = bytearray(valid.data) + dat[index] ^= 0x1 + self.assertFalse(self._tx(common.make_msg(1, 0x2B0, 8, dat)), index) + + def test_brake_command_limits_and_format(self): + self.safety.set_controls_allowed(False) + self.assertTrue(self._tx(self._brake_msg(0, active=False, brake_mode=False))) + self.assertFalse(self._tx(self._brake_msg(1))) + + self.safety.set_controls_allowed(True) + for pressure in (1, 200, 659): + self.assertTrue(self._tx(self._brake_msg(pressure))) + self.assertFalse(self._tx(self._brake_msg(660))) + self.assertFalse(self._tx(self._brake_msg(0, active=True))) + self.assertFalse(self._tx(self._brake_msg(1, active=False))) + self.assertFalse(self._tx(self._brake_msg(0, active=False, brake_mode=True))) + + bad_checksum = self._brake_msg(200) + dat = bytearray(bad_checksum.data) + dat[7] ^= 0x1 + self.assertFalse(self._tx(common.make_msg(1, 0x1C3, 8, dat))) + + def test_gas_override_blocks_longitudinal_commands(self): + self.safety.set_controls_allowed(True) + self._rx(self._user_gas_msg(self.GAS_PRESSED_THRESHOLD + 1)) + self.assertFalse(self._tx(self._accel_msg(6144))) + self.assertFalse(self._tx(self._brake_msg(1))) + self.assertTrue(self._tx(self._accel_msg(5320, active=False))) + self.assertTrue(self._tx(self._brake_msg(0, active=False, brake_mode=False))) + + def test_tester_present(self): + tester_present = make_tester_present_msg(0x707, 1, suppress_response=True) + self.assertTrue(self._tx(self._make_msg(tester_present))) + + for index in range(8): + dat = bytearray(tester_present.dat) + dat[index] ^= 0x1 + self.assertFalse(self._tx(common.make_msg(1, 0x707, 8, dat)), index) + + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index 9afb8e4e9..65291ac7d 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -343,17 +343,15 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase): FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM ALT_MAIN_BUS = SUBARU_ALT_BUS - TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_MAIN_BUS], + TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_CAM_BUS], [SubaruMsg.ES_DashStatus, SUBARU_CAM_BUS], [SubaruMsg.ES_LKAS_State, SUBARU_CAM_BUS], [SubaruMsg.ES_Infotainment, SUBARU_CAM_BUS], [SubaruMsg.ES_Distance, SUBARU_ALT_BUS]] - RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE,), - SUBARU_CAM_BUS: (SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, - SubaruMsg.ES_Infotainment)} + RELAY_MALFUNCTION_ADDRS = {SUBARU_CAM_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus, + SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)} FWD_BLACKLISTED_ADDRS = { - SUBARU_MAIN_BUS: [SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment], - SUBARU_CAM_BUS: [SubaruMsg.ES_LKAS_ANGLE], + SUBARU_MAIN_BUS: [SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment], } def _torque_driver_msg(self, torque): @@ -367,10 +365,10 @@ class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, Test self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY)) self.angle_cmd_cnt += 1 values = {"LKAS_Output": angle, "LKAS_Request": enabled, "SET_3": 3} - return self.packer.make_can_msg_safety("ES_LKAS_ANGLE", SUBARU_MAIN_BUS, values) + return self.packer.make_can_msg_safety("ES_LKAS_ANGLE", SUBARU_CAM_BUS, values) def _angle_meas_msg(self, angle): - return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle}) + return self.packer.make_can_msg_safety("Steering_2", SUBARU_CAM_BUS, {"Steering_Angle": angle}) class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase): diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index a5b40040d..f3df75ce4 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -180,6 +180,9 @@ class CarSpecificEvents: elif self.CP.brand == 'hyundai': events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise, allow_button_cancel=False) + elif self.CP.brand == 'nissan': + events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise) + else: events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index c1d31b3d3..cfbef1016 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -370,9 +370,10 @@ class Car: # and StarPilotCarParams (pandad ORs both safetyParams together) # Use the pre-init longitudinal state here, since Hyundai init() may already # flip CP.openpilotLongitudinalControl to False as part of the fallback. - if was_openpilot_long and self.params.get_bool("EcuDisableFailed"): + if was_openpilot_long and self.CP.brand in ("hyundai", "nissan") and self.params.get_bool("EcuDisableFailed"): # ECU disable failed/rejected - switch to lateral-only mode with stock ACC - LONG_FLAG = 4 # HyundaiSafetyFlags.LONG + # Keep this local to avoid importing every brand's values into card.py. + LONG_FLAG = 4 if self.CP.brand == "hyundai" else 2 if self.CP.brand == "nissan" else 0 for cfg in self.CP.safetyConfigs: cfg.safetyParam &= ~LONG_FLAG for cfg in self.FPCP.safetyConfigs: diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 897d09925..9371d730c 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -323,6 +323,7 @@ class LatControlTorque(LatControl): kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_active else 1.0 kia_carnival_center_taper = get_kia_carnival_center_taper_scale(setpoint, CS.vEgo) if kia_carnival_active else 1.0 tucson_4th_gen_center_taper = get_tucson_4th_gen_center_taper_scale(setpoint, CS.vEgo) if tucson_4th_gen_active else 1.0 + palisade_center_taper = get_palisade_center_taper_scale(setpoint, CS.vEgo) if palisade_active else 1.0 silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active() @@ -353,9 +354,10 @@ class LatControlTorque(LatControl): friction_threshold = get_genesis_g90_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_genesis_g90_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif palisade_active: - ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * palisade_center_taper friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = 1.0 + ((friction_scale - 1.0) * palisade_center_taper) elif prius_active: ff *= get_prius_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * prius_center_taper friction_threshold = get_prius_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index eef2d7990..dd6fadf85 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -170,15 +170,15 @@ RAM_1500_CARS = ( RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT = 1.20 -GENESIS_GV70_FRICTION_THRESHOLD_GAIN = 0.08 +GENESIS_GV70_FRICTION_THRESHOLD_GAIN = 0.12 GENESIS_GV70_FRICTION_SPEED_ONSET = 8.0 * CV.MPH_TO_MS GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS -GENESIS_GV70_FRICTION_SPEED_CUTOFF = 40.0 * CV.MPH_TO_MS -GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH = 6.0 * CV.MPH_TO_MS -GENESIS_GV70_FRICTION_CENTER_LAT = 0.20 -GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.08 -GENESIS_GV70_FRICTION_CALM_JERK = 0.30 -GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.08 +GENESIS_GV70_FRICTION_SPEED_CUTOFF = 60.0 * CV.MPH_TO_MS +GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH = 10.0 * CV.MPH_TO_MS +GENESIS_GV70_FRICTION_CENTER_LAT = 0.28 +GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12 +GENESIS_GV70_FRICTION_CALM_JERK = 0.35 +GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10 BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 @@ -495,6 +495,11 @@ PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08 PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12 PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20 +PALISADE_CENTER_TAPER_MAX = 0.12 +PALISADE_CENTER_TAPER_LAT = 0.28 +PALISADE_CENTER_TAPER_LAT_WIDTH = 0.055 +PALISADE_CENTER_TAPER_SPEED = 12.0 +PALISADE_CENTER_TAPER_SPEED_WIDTH = 2.5 GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 GENESIS_G90_FF_GAIN_LEFT = 0.32 @@ -2311,6 +2316,13 @@ def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desi return min(max(friction_scale, 0.92), 1.12) +def get_palisade_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_TAPER_SPEED) / PALISADE_CENTER_TAPER_SPEED_WIDTH) + center_weight = _palisade_sigmoid((PALISADE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + PALISADE_CENTER_TAPER_LAT_WIDTH) + return 1.0 - (PALISADE_CENTER_TAPER_MAX * speed_weight * center_weight) + + def genesis_g90_lateral_testing_ground_active() -> bool: return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index ec97995f1..ce6dc1dd4 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -66,6 +66,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_gv70_friction_threshold, get_elantra_non_scc_ff_scale, get_palisade_ff_scale, + get_palisade_center_taper_scale, get_palisade_friction_scale, get_palisade_friction_threshold, get_prius_ff_scale, @@ -655,6 +656,11 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind + def test_palisade_center_taper_curve(self): + assert get_palisade_center_taper_scale(0.0, 25.0) < get_palisade_center_taper_scale(0.0, 8.0) + assert get_palisade_center_taper_scale(0.0, 25.0) < get_palisade_center_taper_scale(0.28, 25.0) + assert get_palisade_center_taper_scale(0.28, 25.0) < get_palisade_center_taper_scale(0.6, 25.0) + def test_prius_ff_scale_curve(self): assert get_prius_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_prius_ff_scale(0.7, 0.0, 8.0) @@ -714,11 +720,14 @@ class TestLatControl: base = get_hkg_canfd_base_friction_threshold(12.0) center = get_genesis_gv70_friction_threshold(12.0, 0.0, 0.0) turn = get_genesis_gv70_friction_threshold(12.0, 0.7, 0.8) - high_speed_center = get_genesis_gv70_friction_threshold(40.0, 0.0, 0.0) + highway_center = get_genesis_gv70_friction_threshold(25.0, 0.0, 0.0) + highway_turn = get_genesis_gv70_friction_threshold(25.0, 0.7, 0.8) assert center > base assert turn == pytest.approx(base, rel=0.01) - assert high_speed_center < center + assert highway_center > base + assert highway_turn == pytest.approx(base, rel=0.01) + assert highway_center < center def test_ioniq_5_ff_scale_curve(self): assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index a211837be..27c44bf8c 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -63,8 +63,7 @@ def commanded_torque_at_max_for_saturation(CP, output: float) -> bool: def should_loud_blindspot_alert_without_lateral(CS, sm, starpilot_toggles, combined_left_bsm=None, combined_right_bsm=None) -> bool: - if not (getattr(starpilot_toggles, "loud_blindspot_alert", False) and - getattr(starpilot_toggles, "loud_blindspot_alert_when_disengaged", False)): + if not getattr(starpilot_toggles, "loud_blindspot_alert_when_disengaged", False): return False combined_left_bsm = CS.leftBlindspot if combined_left_bsm is None else combined_left_bsm diff --git a/selfdrive/selfdrived/tests/test_blindspot_alerts.py b/selfdrive/selfdrived/tests/test_blindspot_alerts.py index 4f6e12436..342d6e9ec 100644 --- a/selfdrive/selfdrived/tests/test_blindspot_alerts.py +++ b/selfdrive/selfdrived/tests/test_blindspot_alerts.py @@ -30,9 +30,9 @@ def _sm(lane_change_state=LaneChangeState.off, lane_change_direction=LaneChangeD } -def _toggles(enabled=True): +def _toggles(enabled=True, loud_enabled=True): return SimpleNamespace( - loud_blindspot_alert=True, + loud_blindspot_alert=loud_enabled, loud_blindspot_alert_when_disengaged=enabled, ) @@ -53,6 +53,12 @@ def test_loud_blindspot_alert_accepts_combined_vision_state(): ) +def test_loud_blindspot_alert_without_lateral_is_independent_of_active_loud_alert(): + CS = _car_state(left_blinker=True, left_blindspot=True) + + assert should_loud_blindspot_alert_without_lateral(CS, _sm(), _toggles(loud_enabled=False)) + + def test_loud_blindspot_alert_without_lateral_ignores_active_lateral(): CS = _car_state(right_blinker=True, right_blindspot=True) diff --git a/selfdrive/ui/layouts/settings/starpilot/sounds.py b/selfdrive/ui/layouts/settings/starpilot/sounds.py index 9c480c09c..fd2fdd778 100644 --- a/selfdrive/ui/layouts/settings/starpilot/sounds.py +++ b/selfdrive/ui/layouts/settings/starpilot/sounds.py @@ -321,8 +321,8 @@ class StarPilotSoundsLayout(_SettingsPage): "LoudBlindspotAlertWhenDisengaged": { "title": tr_noop("Loud While Paused"), "subtitle": "", - "is_enabled": lambda: starpilot_state.car_state.hasBSM and self._params.get_bool("LoudBlindspotAlert"), - "disabled_label": tr_noop("Enable Loud Blindspot") + "is_enabled": lambda: starpilot_state.car_state.hasBSM, + "disabled_label": tr_noop("Needs BSM") }, "SpeedLimitChangedAlert": { "title": tr_noop("Speed Limit"), diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 1803358bb..f59a0c771 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -847,14 +847,13 @@ class StarPilotVariables: toggle.csc_no_lead = self.get_value("CurveSpeedControllerNoLead", condition=toggle.curve_speed_controller) toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode - custom_alerts = self.get_value("CustomAlerts") - toggle.goat_scream_alert = self.get_value("GoatScream", condition=custom_alerts) - toggle.goat_scream_critical_alerts = self.get_value("GoatScreamCriticalAlerts", condition=custom_alerts) - toggle.green_light_alert = self.get_value("GreenLightAlert", condition=custom_alerts) - toggle.lead_departing_alert = self.get_value("LeadDepartingAlert", condition=custom_alerts) - toggle.loud_blindspot_alert = self.get_value("LoudBlindspotAlert", condition=custom_alerts and has_bsm) - toggle.loud_blindspot_alert_when_disengaged = self.get_value("LoudBlindspotAlertWhenDisengaged", condition=toggle.loud_blindspot_alert) - toggle.speed_limit_changed_alert = self.get_value("SpeedLimitChangedAlert", condition=custom_alerts) + toggle.goat_scream_alert = self.get_value("GoatScream") + toggle.goat_scream_critical_alerts = self.get_value("GoatScreamCriticalAlerts") + toggle.green_light_alert = self.get_value("GreenLightAlert") + toggle.lead_departing_alert = self.get_value("LeadDepartingAlert") + toggle.loud_blindspot_alert = self.get_value("LoudBlindspotAlert", condition=has_bsm) + toggle.loud_blindspot_alert_when_disengaged = self.get_value("LoudBlindspotAlertWhenDisengaged", condition=has_bsm) + toggle.speed_limit_changed_alert = self.get_value("SpeedLimitChangedAlert") toggle.custom_personalities = toggle.openpilot_longitudinal and self.get_value("CustomPersonalities") toggle.aggressive_jerk_acceleration = self.get_value("AggressiveJerkAcceleration", cast=float, condition=toggle.custom_personalities, conversion=0.01, min=0.25, max=2.0) diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index 8b57594c5..45981343b 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -2613,22 +2613,12 @@ "parent_key": "AlertVolumeControl", "settings_tier": "simple" }, - { - "key": "CustomAlerts", - "label": "StarPilot Alerts", - "description": "Optional StarPilot alerts that highlight driving events in a more noticeable way.", - "data_type": "bool", - "ui_type": "toggle", - "is_parent_toggle": true, - "settings_tier": "simple" - }, { "key": "GoatScream", "label": "Goat Scream", "description": "Play the infamous \"Goat Scream\" when the steering controller reaches its limit. Based on the \"Turn Exceeds Steering Limit\" event.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" }, { @@ -2637,7 +2627,6 @@ "description": "Play the infamous \"Goat Scream\" for full-screen critical alerts that require immediate takeover.\n\nExamples include: \"TAKE CONTROL IMMEDIATELY\" and \"Stock AEB: Risk of Collision\".", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" }, { @@ -2646,7 +2635,6 @@ "description": "Play an alert when the model predicts a red light has turned green.\n\nDisclaimer: openpilot does not explicitly detect traffic lights. This alert is based on end-to-end model predictions from camera input and may trigger even when the light has not changed.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" }, { @@ -2655,7 +2643,6 @@ "description": "Play an alert when the lead vehicle departs from a stop.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" }, { @@ -2664,7 +2651,6 @@ "description": "Play a louder alert if a vehicle is in the blind spot when attempting to change lanes. Based on the \"Car Detected in Blindspot\" event.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" }, { @@ -2673,7 +2659,6 @@ "description": "Play the loud blind spot alert while lateral control is off or paused. Useful when steering pauses on turn signal, since the lane-change state machine is inactive then.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" }, { @@ -2682,7 +2667,6 @@ "description": "Play an alert when the posted speed limit changes.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "CustomAlerts", "settings_tier": "simple" } ] diff --git a/tools/StarPilot/generate_galaxy_layout.py b/tools/StarPilot/generate_galaxy_layout.py index 9d6244c8f..4b1230966 100755 --- a/tools/StarPilot/generate_galaxy_layout.py +++ b/tools/StarPilot/generate_galaxy_layout.py @@ -61,6 +61,7 @@ INJECTED_SECTION_PARAMS = { # Keys explicitly hidden from The Galaxy's generic settings UI. HIDDEN_KEYS = { + "CustomAlerts", "HumanAcceleration", "HideLeadMarker", "HideSpeedLimit", @@ -199,7 +200,6 @@ PARENT_KEYS_MAPPING = { }, "sounds_settings.cc": { "alertVolumeControlKeys": "AlertVolumeControl", - "customAlertsKeys": "CustomAlerts" }, "theme_settings.cc": { "customThemeKeys": "CustomTheme"