diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index f1f32bdca..4149dec39 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -58,6 +58,20 @@ class CarState(CarStateBase): self.active_mode = 0 self.drive_mode_prev = 0 + # Traffic signals for Speed Limit Controller - Credit goes to Multikyd! + def calculate_speed_limit(self, cp, cp_cam): + if self.CP.carFingerprint in CANFD_CAR: + speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam + return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"] + else: + if "SpeedLim_Nav_Clu" in cp.vl["Navi_HU"]: + speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] + elif "CF_Lkas_TsrSpeed_Display_Clu" in cp_cam.vl["LKAS12"]: + speed_limit = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"] + else: + return 0 + return speed_limit if speed_limit not in (0, 255) else 0 + def update(self, cp, cp_cam, frogpilot_toggles): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam, frogpilot_toggles) @@ -176,6 +190,8 @@ class CarState(CarStateBase): self.main_enabled = not self.main_enabled # FrogPilot CarState functions + fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp, cp_cam) * speed_conv + self.prev_distance_button = self.distance_button self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST @@ -270,6 +286,8 @@ class CarState(CarStateBase): else cp_cam.vl["CAM_0x2a4"]) # FrogPilot CarState functions + fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp, cp_cam) * speed_factor + self.prev_distance_button = self.distance_button self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST @@ -338,6 +356,10 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.CAN_LFA_BTN: messages.append(("BCM_PO_11", 50)) + messages += [ + ("Navi_HU", 5), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod @@ -358,6 +380,8 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.USE_FCA.value: messages.append(("FCA11", 50)) + messages.append(("LKAS12", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) def get_can_parser_canfd(self, CP): @@ -394,6 +418,9 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] + if CP.flags & HyundaiFlags.CANFD_HDA2: + messages.append(("CLUSTER_SPEED_LIMIT", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN) @staticmethod @@ -407,4 +434,7 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] + if not (CP.flags & HyundaiFlags.CANFD_HDA2): + messages.append(("CLUSTER_SPEED_LIMIT", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e6e05fd3d..102647d48 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -27,6 +27,22 @@ PERM_STEER_FAULTS = (3, 17) ZSS_THRESHOLD = 4.0 ZSS_THRESHOLD_COUNT = 10 +# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! +@staticmethod +def calculate_speed_limit(cp_cam, frogpilot_toggles): + signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] + traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} + + tsgn1 = traffic_signals.get("TSGN1", None) + spdval1 = traffic_signals.get("SPDVAL1", None) + + if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: + return spdval1 * CV.KPH_TO_MS + elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: + return spdval1 * CV.MPH_TO_MS + else: + return 0 + class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) @@ -196,6 +212,8 @@ class CarState(CarStateBase): self.cruise_increased_previously = self.cruise_increased self.cruise_increased = self.pcm_acc_status == 9 + fp_ret.dashboardSpeedLimit = calculate_speed_limit(cp_cam, frogpilot_toggles) + fp_ret.ecoGear = cp.vl["GEAR_PACKET"]['ECON_ON'] == 1 fp_ret.sportGear = cp.vl["GEAR_PACKET"]['SPORT_ON_2' if self.CP.flags & ToyotaFlags.NO_DSU else 'SPORT_ON'] == 1 @@ -291,6 +309,11 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): messages = [] + messages += [ + ("RSA1", 0), + ("RSA2", 0), + ] + if CP.carFingerprint != CAR.TOYOTA_PRIUS_V: messages += [ ("LKAS_HUD", 1), diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index ce200c589..800f25064 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -238,7 +238,7 @@ class FrogPilotPlanner: # Pfeiferj's Speed Limit Controller if frogpilot_toggles.speed_limit_controller: - SpeedLimitController.update(controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles) + SpeedLimitController.update(frogpilotCarState.dashboardSpeedLimit, controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles) unconfirmed_slc_target = SpeedLimitController.desired_speed_limit if frogpilot_toggles.speed_limit_confirmation and self.slc_target != 0: diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py index 8d6b08040..bba906f87 100644 --- a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -24,6 +24,7 @@ class SpeedLimitController: self.params = Params() self.params_memory = Params("/dev/shm/params") + self.car_speed_limit = 0 # m/s self.map_speed_limit = 0 # m/s self.max_speed_limit = 0 # m/s self.nav_speed_limit = 0 # m/s @@ -40,7 +41,8 @@ class SpeedLimitController: self.params.put_float_nonblocking("PreviousSpeedLimit", speed_limit) self.prv_speed_limit = speed_limit - def update(self, enabled, navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles): + def update(self, dashboardSpeedLimit, enabled, navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles): + self.car_speed_limit = dashboardSpeedLimit self.write_map_state(v_ego) self.nav_speed_limit = navigationSpeedLimit @@ -94,7 +96,7 @@ class SpeedLimitController: @property def speed_limit(self): - limits = [self.map_speed_limit, self.nav_speed_limit] + limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit] filtered_limits = [float(limit) for limit in limits if limit > 1] if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits: @@ -103,6 +105,7 @@ class SpeedLimitController: return min(filtered_limits) speed_limits = { + "Dashboard": self.car_speed_limit, "Offline Maps": self.map_speed_limit, "Navigation": self.nav_speed_limit, }