diff --git a/opendbc/car/subaru/carcontroller.py b/opendbc/car/subaru/carcontroller.py index 4ee3655268..1be4735774 100644 --- a/opendbc/car/subaru/carcontroller.py +++ b/opendbc/car/subaru/carcontroller.py @@ -1,6 +1,6 @@ import copy from opendbc.can.packer import CANPacker -from opendbc.car import apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg +from opendbc.car import apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, apply_std_steer_angle_limits from opendbc.car.common.numpy_fast import clip, interp from opendbc.car.interfaces import CarControllerBase from opendbc.car.subaru import subarucan @@ -32,28 +32,38 @@ def update(self, CC, CS, now_nanos): # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: - apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) + apply_steer = 0 + if self.CP.flags & SubaruFlags.LKAS_ANGLE: + apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, self.p) - # limits due to driver torque + if not CC.latActive: + apply_steer = CS.out.steeringAngleDeg - new_steer = int(round(apply_steer)) - apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) + can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) - if not CC.latActive: - apply_steer = 0 - - if self.CP.flags & SubaruFlags.PREGLOBAL: - can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive)) else: - apply_steer_req = CC.latActive + apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) + + # limits due to driver torque - if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED: - # Steering rate fault prevention - self.steer_rate_counter, apply_steer_req = \ - common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req, - self.steer_rate_counter, MAX_STEER_RATE_FRAMES) + new_steer = int(round(apply_steer)) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) - can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req)) + if not CC.latActive: + apply_steer = 0 + + if self.CP.flags & SubaruFlags.PREGLOBAL: + can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive)) + else: + apply_steer_req = CC.latActive + + if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED: + # Steering rate fault prevention + self.steer_rate_counter, apply_steer_req = \ + common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req, + self.steer_rate_counter, MAX_STEER_RATE_FRAMES) + + can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req)) self.apply_steer_last = apply_steer @@ -137,8 +147,11 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_es_static_2(self.packer)) new_actuators = copy.copy(actuators) - new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX - new_actuators.steerOutputCan = self.apply_steer_last + if self.CP.flags & SubaruFlags.LKAS_ANGLE: + new_actuators.steeringAngleDeg = self.apply_steer_last + else: + new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last self.frame += 1 return new_actuators, can_sends diff --git a/opendbc/car/subaru/carstate.py b/opendbc/car/subaru/carstate.py index 84fa215815..b721f4f776 100644 --- a/opendbc/car/subaru/carstate.py +++ b/opendbc/car/subaru/carstate.py @@ -75,12 +75,18 @@ def update(self, cp, cp_cam, _, cp_body, __) -> structs.CarState: ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold cp_cruise = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp - if self.CP.flags & SubaruFlags.HYBRID: + if self.CP.flags & SubaruFlags.LKAS_ANGLE: ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 + else: - ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 - ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 + if self.CP.flags & SubaruFlags.HYBRID: + ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 + ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 + else: + ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 + ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 + ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS if (self.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \ diff --git a/opendbc/car/subaru/fingerprints.py b/opendbc/car/subaru/fingerprints.py index 3c61915fc5..1ffeaf074b 100644 --- a/opendbc/car/subaru/fingerprints.py +++ b/opendbc/car/subaru/fingerprints.py @@ -42,13 +42,17 @@ ], (Ecu.fwdCamera, 0x787, None): [ b'\x05!\x08\x1dK\x05!\x08\x01/', - ], - (Ecu.engine, 0x7a2, None): [ - b'\xe5,\xa0P\x07', - ], - (Ecu.transmission, 0x7a3, None): [ - b'\x04\xfe\xf3\x00\x00', - ], + b'\x05!\x08\x1dK\x00\x00\x00\x00\x00', + ], + # XXX: No longer showing up?! + #(Ecu.engine, 0x7a2, None): [ + # b'\xe5,\xa0P\x07', + # b'\xe5,\xa0p\x07', + #], + #(Ecu.transmission, 0x7a3, None): [ + # b'\x04\xfe\xf3\x00\x00', + # b'\x04\xfe\xf6\x00\x00', + #], }, CAR.SUBARU_LEGACY: { (Ecu.abs, 0x7b0, None): [ diff --git a/opendbc/car/subaru/interface.py b/opendbc/car/subaru/interface.py index 782f73e8c6..cb9e1b9261 100644 --- a/opendbc/car/subaru/interface.py +++ b/opendbc/car/subaru/interface.py @@ -30,6 +30,8 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, exp ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)] if ret.flags & SubaruFlags.GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 + if ret.flags & SubaruFlags.LKAS_ANGLE: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LKAS_ANGLE ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 @@ -39,13 +41,23 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, exp else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023): + if candidate == CAR.SUBARU_ASCENT: ret.steerActuatorDelay = 0.3 # end-to-end angle controller ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.00003 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]] + elif candidate == CAR.SUBARU_ASCENT_2023: + ret.dashcamOnly = False + ret.steerActuatorDelay = 0.3 # end-to-end angle controller + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kf = 0.00003 + ret.lateralTuning.pid.kpBP = [0., 20.] + ret.lateralTuning.pid.kiBP = [0., 20.] + ret.lateralTuning.pid.kpV = [0.0025, 0.1] + ret.lateralTuning.pid.kiV = [0.00225, 0.09] + elif candidate == CAR.SUBARU_IMPREZA: ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.lateralTuning.init('pid') diff --git a/opendbc/car/subaru/values.py b/opendbc/car/subaru/values.py index c7f93f9fae..84ebb178a0 100644 --- a/opendbc/car/subaru/values.py +++ b/opendbc/car/subaru/values.py @@ -2,7 +2,7 @@ from enum import Enum, IntFlag from panda import uds -from opendbc.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from opendbc.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict, AngleRateLimit from opendbc.car.structs import CarParams from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 @@ -53,6 +53,9 @@ def __init__(self, CP): BRAKE_LOOKUP_BP = [-3.5, 0] BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN] + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0.], angle_v=[1.]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0.], angle_v=[1.]) + class SubaruFlags(IntFlag): # Detected flags @@ -271,7 +274,7 @@ class CAR(Platforms): # We don't get the EPS from non-OBD queries on GEN2 cars. Note that we still attempt to match when it exists non_essential_ecus={ Ecu.eps: list(CAR.with_flags(SubaruFlags.GLOBAL_GEN2)), - } + }, ) DBC = CAR.create_dbc_map()