diff --git a/.gitmodules b/.gitmodules index 26f93ef164e782..de7fbc4cf80f4a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "panda"] path = panda - url = ../../commaai/panda.git + url = ../../martinl/panda.git + branch = subaru-long-sng [submodule "opendbc"] path = opendbc url = ../../commaai/opendbc.git diff --git a/panda b/panda index 4160d8d71cbb5c..590a8319d07df9 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 4160d8d71cbb5cd40107834ad4756c4dbffc63b0 +Subproject commit 590a8319d07df9fcc9b2dea2aed1940a2b447081 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index c4246b3806c40c..aa9c430bf74cda 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -15,7 +15,10 @@ def __init__(self, dbc_name, CP, VM): self.es_dashstatus_cnt = -1 self.infotainmentstatus_cnt = -1 self.cruise_button_prev = 0 + self.prev_cruise_state = 0 self.last_cancel_frame = 0 + self.throttle_cnt = -1 + self.manual_hold = False self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) @@ -47,6 +50,17 @@ def update(self, CC, CS, now_nanos): self.apply_steer_last = apply_steer + # *** stop and go *** + + if self.CP.carFingerprint not in PREGLOBAL_CARS: + # Record manual hold set while in standstill and no car in front + if CS.out.standstill and self.prev_cruise_state == 1 and CS.cruise_state == 3 and CS.car_follow == 0: + self.manual_hold = True + if not CS.out.standstill: + self.manual_hold = False + self.prev_cruise_state = CS.cruise_state + + throttle_cmd = True if CC.enabled and CC.cruiseControl.resume and not self.manual_hold else False # *** alerts and pcm cancel *** @@ -70,6 +84,10 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) self.es_distance_cnt = CS.es_distance_msg["COUNTER"] + if self.throttle_cnt != CS.throttle_msg["COUNTER"]: + can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg, throttle_cmd)) + self.throttle_cnt = CS.throttle_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 @@ -90,6 +108,10 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_infotainmentstatus(self.packer, CS.es_infotainmentstatus_msg, hud_control.visualAlert)) self.infotainmentstatus_cnt = CS.es_infotainmentstatus_msg["COUNTER"] + if self.throttle_cnt != CS.throttle_msg["COUNTER"]: + can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd)) + self.throttle_cnt = CS.throttle_msg["COUNTER"] + 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 9d7b0a65cc7a39..4f29c64e228eb8 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -78,8 +78,11 @@ def update(self, cp, cp_cam, cp_body): ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2 self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) + self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"] + self.throttle_msg = copy.copy(cp.vl["Throttle"]) cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + self.car_follow = cp_es_distance.vl["ES_Distance"]["Car_Follow"] self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: @@ -180,6 +183,16 @@ def get_can_parser(CP): checks += CarState.get_common_global_signals()[1] signals += [ + ("COUNTER", "Throttle"), + ("Signal1", "Throttle"), + ("Engine_RPM", "Throttle"), + ("Signal2", "Throttle"), + ("Throttle_Pedal", "Throttle"), + ("Throttle_Cruise", "Throttle"), + ("Throttle_Combo", "Throttle"), + ("Signal1", "Throttle"), + ("Off_Accel", "Throttle"), + ("Steer_Warning", "Steering_Torque"), ("UNITS", "Dashlights"), ] @@ -190,6 +203,20 @@ def get_can_parser(CP): ] else: signals += [ + ("Throttle_Pedal", "Throttle"), + ("COUNTER", "Throttle"), + ("Signal1", "Throttle"), + ("Not_Full_Throttle", "Throttle"), + ("Signal2", "Throttle"), + ("Engine_RPM", "Throttle"), + ("Off_Throttle", "Throttle"), + ("Signal3", "Throttle"), + ("Throttle_Cruise", "Throttle"), + ("Throttle_Combo", "Throttle"), + ("Throttle_Body", "Throttle"), + ("Off_Throttle_2", "Throttle"), + ("Signal4", "Throttle"), + ("FL", "Wheel_Speeds"), ("FR", "Wheel_Speeds"), ("RL", "Wheel_Speeds"), diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 3b498d3f701fc9..f3f6ec51de63f9 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -67,7 +67,6 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_ return packer.make_can_msg("ES_LKAS_State", 0, values) - def create_es_dashstatus(packer, dashstatus_msg): values = copy.copy(dashstatus_msg) @@ -77,7 +76,6 @@ def create_es_dashstatus(packer, dashstatus_msg): return packer.make_can_msg("ES_DashStatus", 0, values) - def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts if infotainmentstatus_msg["LKAS_State_Infotainment"] in (3, 4): @@ -93,6 +91,13 @@ def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): return packer.make_can_msg("INFOTAINMENT_STATUS", 0, infotainmentstatus_msg) +def create_throttle(packer, throttle_msg, throttle_cmd): + + values = copy.copy(throttle_msg) + if throttle_cmd: + values["Throttle_Pedal"] = 5 + + return packer.make_can_msg("Throttle", 2, values) # *** Subaru Pre-global *** @@ -118,3 +123,13 @@ def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance") return packer.make_can_msg("ES_Distance", 0, values) + +def create_preglobal_throttle(packer, throttle_msg, throttle_cmd): + + values = copy.copy(throttle_msg) + if throttle_cmd: + values["Throttle_Pedal"] = 5 + + values["Checksum"] = subaru_preglobal_checksum(packer, values, "Throttle") + + return packer.make_can_msg("Throttle", 2, values)