diff --git a/.gitmodules b/.gitmodules index 26f93ef164..2302266243 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "panda"] path = panda - url = ../../commaai/panda.git + url = ../../martinl/panda.git + branch = feature-subaru-long [submodule "opendbc"] path = opendbc url = ../../commaai/opendbc.git diff --git a/panda b/panda index 1923b14189..0575d7a33d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 1923b1418933e464ff7460a3e0a05d63aad0d53b +Subproject commit 0575d7a33d2ec2d8f7c6084c9f2eda264f55ea0f diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a6dbf4a39e..436441bf84 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,8 +1,24 @@ +from common.numpy_fast import clip from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams +ACCEL_HYST_GAP = 10 # don't change accel command for small oscilalitons within this value + +def accel_hysteresis(accel, accel_steady): + + # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command + if accel > accel_steady + ACCEL_HYST_GAP: + accel_steady = accel - ACCEL_HYST_GAP + elif accel < accel_steady - ACCEL_HYST_GAP: + accel_steady = accel + ACCEL_HYST_GAP + accel = accel_steady + + return accel, accel_steady + +def compute_gb(accel): + return clip(accel/4.0, 0.0, 1.0), clip(-accel/4.0, 0.0, 1.0) class CarController: def __init__(self, dbc_name, CP, VM): @@ -10,10 +26,20 @@ class CarController: self.apply_steer_last = 0 self.frame = 0 - self.es_lkas_cnt = -1 + self.es_lkas_state_cnt = -1 self.es_distance_cnt = -1 self.es_dashstatus_cnt = -1 + self.cruise_control_cnt = -1 + self.brake_status_cnt = -1 + self.es_status_cnt = -1 + self.es_brake_cnt = -1 self.cruise_button_prev = 0 + self.steer_rate_limited = False + self.cruise_rpm_last = 0 + self.cruise_throttle_last = 0 + self.rpm_steady = 0 + self.throttle_steady = 0 + self.last_cancel_frame = 0 self.p = CarControllerParams(CP) @@ -46,6 +72,40 @@ class CarController: self.apply_steer_last = apply_steer + ### LONG ### + + cruise_rpm = 0 + cruise_throttle = 0 + + brake_cmd = False + brake_value = 0 + + if self.CP.openpilotLongitudinalControl: + + gas, brake = compute_gb(actuators.accel) + + if CC.longActive and brake > 0: + brake_value = clip(int(brake * CarControllerParams.BRAKE_SCALE), CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX) + brake_cmd = True + + # AEB passthrough + if CC.enabled and CS.aeb: + brake_cmd = False + + if CC.longActive and gas > 0: + # limit min and max values + cruise_throttle = clip(int(CarControllerParams.THROTTLE_BASE + (gas * CarControllerParams.THROTTLE_SCALE)), CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX) + cruise_rpm = clip(int(CarControllerParams.RPM_BASE + (gas * CarControllerParams.RPM_SCALE)), CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX) + # hysteresis + cruise_throttle, self.throttle_steady = accel_hysteresis(cruise_throttle, self.throttle_steady) + cruise_rpm, self.rpm_steady = accel_hysteresis(cruise_rpm, self.rpm_steady) + + # slow down the signals change + cruise_throttle = clip(cruise_throttle, self.cruise_throttle_last - CarControllerParams.THROTTLE_DELTA, self.cruise_throttle_last + CarControllerParams.THROTTLE_DELTA) + cruise_rpm = clip(cruise_rpm, self.cruise_rpm_last - CarControllerParams.RPM_DELTA, self.cruise_rpm_last + CarControllerParams.RPM_DELTA) + + self.cruise_throttle_last = cruise_throttle + self.cruise_rpm_last = cruise_rpm # *** alerts and pcm cancel *** @@ -70,20 +130,42 @@ class CarController: self.es_distance_cnt = CS.es_distance_msg["COUNTER"] else: - if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2: - bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0 - 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.es_dashstatus_cnt != CS.es_dashstatus_msg["COUNTER"]: - 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, CC.longActive, hud_control.leadVisible)) 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, + if self.es_lkas_state_cnt != CS.es_lkas_state_msg["COUNTER"]: + 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, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) - self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"] + self.es_lkas_state_cnt = CS.es_lkas_state_msg["COUNTER"] + + if self.CP.openpilotLongitudinalControl: + if self.es_status_cnt != CS.es_status_msg["COUNTER"]: + can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, CC.longActive, cruise_rpm)) + self.es_status_cnt = CS.es_status_msg["COUNTER"] + + if self.es_brake_cnt != CS.es_brake_msg["COUNTER"]: + can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, brake_cmd, brake_value)) + self.es_brake_cnt = CS.es_brake_msg["COUNTER"] + + if self.cruise_control_cnt != CS.cruise_control_msg["COUNTER"]: + can_sends.append(subarucan.create_cruise_control(self.packer, CS.cruise_control_msg)) + self.cruise_control_cnt = CS.cruise_control_msg["COUNTER"] + + if self.brake_status_cnt != CS.brake_status_msg["COUNTER"]: + can_sends.append(subarucan.create_brake_status(self.packer, CS.brake_status_msg, CS.aeb)) + self.brake_status_cnt = CS.brake_status_msg["COUNTER"] + + if self.es_distance_cnt != CS.es_distance_msg["COUNTER"]: + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd, CC.longActive, brake_cmd, brake_value, cruise_throttle)) + self.es_distance_cnt = CS.es_distance_msg["COUNTER"] + + else: + if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2: + bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0 + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd, CC.longActive, brake_cmd, brake_value, cruise_throttle)) + self.last_cancel_frame = self.frame new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ba873c48d7..fcce95a37e 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -69,6 +69,7 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 + if self.car_fingerprint in PREGLOBAL_CARS: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] @@ -77,8 +78,17 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2 + ret.stockAeb = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 5) or \ + (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert_Msg"] == 6) or ret.stockFcw self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) - + 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.aeb = cp_es_brake.vl["ES_Brake"]["Cruise_Brake_Active"] + 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"]) cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) @@ -88,13 +98,21 @@ class CarState(CarStateBase): @staticmethod def get_common_global_signals(): signals = [ + ("COUNTER", "CruiseControl"), + ("Signal1", "CruiseControl"), ("Cruise_On", "CruiseControl"), ("Cruise_Activated", "CruiseControl"), + ("Signal2", "CruiseControl"), ("FL", "Wheel_Speeds"), ("FR", "Wheel_Speeds"), ("RL", "Wheel_Speeds"), ("RR", "Wheel_Speeds"), + ("COUNTER", "Brake_Status"), + ("Signal1", "Brake_Status"), + ("ES_Brake", "Brake_Status"), + ("Signal2", "Brake_Status"), ("Brake", "Brake_Status"), + ("Signal3", "Brake_Status"), ] checks = [ ("CruiseControl", 20), @@ -105,7 +123,7 @@ class CarState(CarStateBase): return signals, checks @staticmethod - def get_global_es_distance_signals(): + def get_global_es_signals(): signals = [ ("COUNTER", "ES_Distance"), ("Signal1", "ES_Distance"), @@ -126,9 +144,32 @@ class CarState(CarStateBase): ("Cruise_Set", "ES_Distance"), ("Cruise_Resume", "ES_Distance"), ("Signal6", "ES_Distance"), + + ("COUNTER", "ES_Status"), + ("Signal1", "ES_Status"), + ("Cruise_Fault", "ES_Status"), + ("Cruise_RPM", "ES_Status"), + ("Signal2", "ES_Status"), + ("Cruise_Activated", "ES_Status"), + ("Brake_Lights", "ES_Status"), + ("Cruise_Hold", "ES_Status"), + ("Signal3", "ES_Status"), + + ("COUNTER", "ES_Brake"), + ("Signal1", "ES_Brake"), + ("Brake_Pressure", "ES_Brake"), + ("Signal2", "ES_Brake"), + ("Cruise_Brake_Lights", "ES_Brake"), + ("Cruise_Brake_Fault", "ES_Brake"), + ("Cruise_Brake_Active", "ES_Brake"), + ("Cruise_Activated", "ES_Brake"), + ("Signal3", "ES_Brake"), + ] checks = [ ("ES_Distance", 20), + ("ES_Status", 20), + ("ES_Brake", 20), ] return signals, checks @@ -143,6 +184,7 @@ class CarState(CarStateBase): ("Steer_Error_1", "Steering_Torque"), ("Brake_Pedal", "Brake_Pedal"), ("Throttle_Pedal", "Throttle"), + ("Throttle_Cruise", "Throttle"), ("LEFT_BLINKER", "Dashlights"), ("RIGHT_BLINKER", "Dashlights"), ("SEATBELT_FL", "Dashlights"), @@ -179,6 +221,7 @@ class CarState(CarStateBase): signals += [ ("Steer_Warning", "Steering_Torque"), + ("RPM", "Transmission"), ("UNITS", "Dashlights"), ] @@ -248,7 +291,7 @@ class CarState(CarStateBase): ] else: signals = [ - ("Counter", "ES_DashStatus"), + ("COUNTER", "ES_DashStatus"), ("PCB_Off", "ES_DashStatus"), ("LDW_Off", "ES_DashStatus"), ("Signal1", "ES_DashStatus"), @@ -290,6 +333,7 @@ class CarState(CarStateBase): ("LKAS_Right_Line_Visible", "ES_LKAS_State"), ("LKAS_Alert", "ES_LKAS_State"), ("Signal3", "ES_LKAS_State"), + ] checks = [ @@ -298,8 +342,8 @@ class CarState(CarStateBase): ] if CP.carFingerprint not in GLOBAL_GEN2: - signals += CarState.get_global_es_distance_signals()[0] - checks += CarState.get_global_es_distance_signals()[1] + signals += CarState.get_global_es_signals()[0] + checks += CarState.get_global_es_signals()[1] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) @@ -307,8 +351,8 @@ class CarState(CarStateBase): def get_body_can_parser(CP): if CP.carFingerprint in GLOBAL_GEN2: signals, checks = CarState.get_common_global_signals() - signals += CarState.get_global_es_distance_signals()[0] - checks += CarState.get_global_es_distance_signals()[1] + signals += CarState.get_global_es_signals()[0] + checks += CarState.get_global_es_signals()[1] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) return None diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 733482ef82..859f8b6af6 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -101,6 +101,19 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"unknown car: {candidate}") + # longitudinal + ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS) + if experimental_long and ret.experimentalLongitudinalAvailable: + 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.openpilotLongitudinalControl = experimental_long + if ret.openpilotLongitudinalControl: + 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 d83b639a41..56ded9f340 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -14,14 +14,23 @@ def create_steering_control(packer, apply_steer): 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_active, brake_cmd, brake_value, cruise_throttle): + values = copy.copy(es_distance_msg) - values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 + if long_active: + values["Cruise_Throttle"] = cruise_throttle if pcm_cancel_cmd: + values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 values["Cruise_Cancel"] = 1 + if brake_cmd: + values["Cruise_Throttle"] = 808 if brake_value >= 35 else 1818 + values["Cruise_Brake_Active"] = 1 + # Do not disable openpilot on Eyesight Soft Disable + values["Cruise_Soft_Disable"] = 0 + 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_state(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): values = copy.copy(es_lkas_msg) @@ -67,8 +76,14 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_ return packer.make_can_msg("ES_LKAS_State", 0, values) -def create_es_dashstatus(packer, dashstatus_msg): - values = copy.copy(dashstatus_msg) +def create_es_dashstatus(packer, es_dashstatus_msg, enabled, long_active, lead_visible): + + values = copy.copy(es_dashstatus_msg) + if enabled and long_active: + values["Cruise_State"] = 0 + values["Cruise_Activated"] = 1 + values["Cruise_Disengaged"] = 0 + values["Car_Follow"] = int(lead_visible) # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts if values["LKAS_State_Msg"] in [2, 3]: @@ -76,6 +91,44 @@ def create_es_dashstatus(packer, dashstatus_msg): return packer.make_can_msg("ES_DashStatus", 0, values) +def create_es_brake(packer, es_brake_msg, enabled, brake_cmd, brake_value): + + values = copy.copy(es_brake_msg) + if enabled: + values["Cruise_Activated"] = 1 + if brake_cmd: + values["Brake_Pressure"] = brake_value + values["Cruise_Brake_Active"] = 1 + values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0 + + return packer.make_can_msg("ES_Brake", 0, values) + +def create_es_status(packer, es_status_msg, long_active, cruise_rpm): + + values = copy.copy(es_status_msg) + if long_active: + values["Cruise_Activated"] = 1 + values["Cruise_RPM"] = cruise_rpm + + return packer.make_can_msg("ES_Status", 0, values) + +# disable cruise_activated feedback to eyesight to keep ready state +def create_cruise_control(packer, cruise_control_msg): + + values = copy.copy(cruise_control_msg) + values["Cruise_Activated"] = 0 + + return packer.make_can_msg("CruiseControl", 2, values) + +# disable es_brake feedback to eyesight, exempt AEB +def create_brake_status(packer, brake_status_msg, aeb): + + values = copy.copy(brake_status_msg) + if not aeb: + values["ES_Brake"] = 0 + + return packer.make_can_msg("Brake_Status", 2, values) + # *** Subaru Pre-global *** def subaru_preglobal_checksum(packer, values, addr): diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 5c7d3f5e4d..3fa4fe5d5d 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -29,6 +29,21 @@ class CarControllerParams: else: self.STEER_MAX = 2047 + RPM_MIN = 0 # min cruise_rpm + RPM_MAX = 3200 # max cruise_rpm + RPM_BASE = 600 # cruise_rpm idle, from stock drive + RPM_SCALE = 3000 # cruise_rpm, from testing + RPM_DELTA = 50 + + THROTTLE_MIN = 0 # min cruise_throttle + THROTTLE_MAX = 3400 # max cruise_throttle + THROTTLE_BASE = 1810 # cruise_throttle, from stock drive + THROTTLE_SCALE = 3000 # from testing + THROTTLE_DELTA = 50 + + BRAKE_MIN = 0 + BRAKE_MAX = 400 + BRAKE_SCALE = 1000 # from testing class CAR: # Global platform