diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index b37c88797a..80634bf261 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,3 +1,4 @@ +from common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan @@ -42,6 +43,21 @@ class CarController: self.apply_steer_last = apply_steer + # *** longitudinal *** + + if CC.longActive: + apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V))) + apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V))) + apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V))) + + # limit min and max values + cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX) + cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX) + cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX) + else: + cruise_throttle = CarControllerParams.THROTTLE_INACTIVE + cruise_rpm = CarControllerParams.RPM_INACTIVE + cruise_brake = CarControllerParams.BRAKE_MIN # *** alerts and pcm cancel *** if self.CP.carFingerprint in PREGLOBAL_CARS: @@ -64,13 +80,9 @@ class CarController: can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) else: - if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2: - bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main - can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd)) - self.last_cancel_frame = self.frame - if self.frame % 10 == 0: - can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg)) + can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl, + CC.longActive, hud_control.leadVisible)) can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, @@ -79,6 +91,20 @@ class CarController: if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert)) + if self.CP.openpilotLongitudinalControl: + if self.frame % 5 == 0: + can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm)) + + can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, cruise_brake)) + + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd, + self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle)) + else: + if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2: + bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd)) + self.last_cancel_frame = self.frame + new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 6e0bef2068..84c1285af1 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -91,6 +91,12 @@ class CarState(CarStateBase): ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \ (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0) self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) + cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"]) + cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"]) + self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"]) + self.brake_status_msg = copy.copy(cp_brakes.vl["Brake_Status"]) self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) @@ -114,6 +120,8 @@ class CarState(CarStateBase): messages = [ ("ES_Brake", 20), ("ES_Distance", 20), + ("ES_Status", 20), + ("ES_Brake", 20), ] return messages diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index b01d1eb3bc..7b532a1b22 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -107,6 +107,18 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"unknown car: {candidate}") + #ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE) + ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable + + if ret.openpilotLongitudinalControl: + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] + + ret.stoppingControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG + return ret # returns a car.CarState diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 39658e958a..df2718c764 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -16,8 +16,7 @@ def create_steering_control(packer, apply_steer, steer_req): def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) - -def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): +def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0): values = {s: es_distance_msg[s] for s in [ "CHECKSUM", "COUNTER", @@ -41,9 +40,20 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): "Signal6", ]} values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 + + if long_enabled: + values["Cruise_Throttle"] = cruise_throttle + + # Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long + values["Cruise_Soft_Disable"] = 0 + + if brake_cmd: + values["Cruise_Brake_Active"] = 1 + if pcm_cancel_cmd: values["Cruise_Cancel"] = 1 values["Cruise_Throttle"] = 1818 # inactive throttle + return packer.make_can_msg("ES_Distance", bus, values) @@ -106,8 +116,7 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_ return packer.make_can_msg("ES_LKAS_State", CanBus.main, values) - -def create_es_dashstatus(packer, dashstatus_msg): +def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_active, lead_visible): values = {s: dashstatus_msg[s] for s in [ "CHECKSUM", "COUNTER", @@ -138,12 +147,68 @@ def create_es_dashstatus(packer, dashstatus_msg): "Cruise_State", ]} + if enabled and long_active: + values["Cruise_State"] = 0 + values["Cruise_Activated"] = 1 + values["Cruise_Disengaged"] = 0 + values["Car_Follow"] = int(lead_visible) + + if long_enabled: + values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash + # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts if values["LKAS_State_Msg"] in (2, 3): values["LKAS_State_Msg"] = 0 return packer.make_can_msg("ES_DashStatus", CanBus.main, values) +def create_es_brake(packer, es_brake_msg, enabled, brake_value): + values = {s: es_brake_msg[s] for s in [ + "CHECKSUM", + "COUNTER", + "Signal1", + "Brake_Pressure", + "AEB_Status", + "Cruise_Brake_Lights", + "Cruise_Brake_Fault", + "Cruise_Brake_Active", + "Cruise_Activated", + "Signal3", + ]} + + if enabled: + values["Cruise_Activated"] = 1 + + values["Brake_Pressure"] = brake_value + + if brake_value > 0: + values["Cruise_Brake_Active"] = 1 + values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0 + + return packer.make_can_msg("ES_Brake", CanBus.main, values) + +def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rpm): + values = {s: es_status_msg[s] for s in [ + "CHECKSUM", + "COUNTER", + "Signal1", + "Cruise_Fault", + "Cruise_RPM", + "Signal2", + "Cruise_Activated", + "Brake_Lights", + "Cruise_Hold", + "Signal3", + ]} + + if long_enabled: + values["Cruise_RPM"] = cruise_rpm + + if long_active: + values["Cruise_Activated"] = 1 + + return packer.make_can_msg("ES_Status", CanBus.main, values) + def create_es_infotainment(packer, es_infotainment_msg, visual_alert): # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index ebeb5ea2a0..dc25f31f73 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -29,6 +29,29 @@ class CarControllerParams: else: self.STEER_MAX = 2047 + THROTTLE_MIN = 808 + THROTTLE_MAX = 3400 + + THROTTLE_INACTIVE = 1818 # corresponds to zero acceleration + THROTTLE_ENGINE_BRAKE = 808 # while braking, eyesight sets throttle to this, probably for engine braking + + BRAKE_MIN = 0 + BRAKE_MAX = 600 # about -3.5m/s2 from testing + + RPM_MIN = 0 + RPM_MAX = 2400 + + RPM_INACTIVE = 600 # a good base rpm for zero acceleration + + THROTTLE_LOOKUP_BP = [0, 1] + THROTTLE_LOOKUP_V = [THROTTLE_INACTIVE, THROTTLE_MAX] + + RPM_LOOKUP_BP = [0, 1] + RPM_LOOKUP_V = [RPM_INACTIVE, RPM_MAX] + + BRAKE_LOOKUP_BP = [-1, 0] + BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN] + class SubaruFlags(IntFlag): SEND_INFOTAINMENT = 1