From aa05d280ab106780ff858ed6ada5c564dd866502 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 28 May 2024 23:48:11 -0700 Subject: [PATCH] Controls - Speed Limit Controller - Read speed limits from the dash Co-Authored-By: Efini <19368997+efini@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com> Co-Authored-By: multikyd <72329880+multikyd@users.noreply.github.com> --- selfdrive/car/hyundai/carstate.py | 25 +++++++++++++++++++ selfdrive/car/toyota/carstate.py | 22 ++++++++++++++++ .../frogpilot/controls/frogpilot_planner.py | 2 +- .../controls/lib/speed_limit_controller.py | 7 ++++-- 4 files changed, 53 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 136d307..ff282a8 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -58,6 +58,17 @@ 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" not in cp.vl["Navi_HU"]: + return 0 + speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] + return speed_limit if speed_limit not in (0, 255) else 0 + def update(self, cp, cp_cam, frogpilot_variables): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam, frogpilot_variables) @@ -175,6 +186,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 @@ -269,6 +282,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 and self.prev_cruise_buttons == 0 @@ -336,6 +351,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 @@ -392,6 +411,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 @@ -405,4 +427,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 c010fce..8dae978 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -58,6 +58,21 @@ class CarState(CarStateBase): self.zss_angle_offset = 0 self.zss_threshold_count = 0 + # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! + def calculate_speed_limit(self, cp_cam, frogpilot_variables): + 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_variables.force_mph_dashboard: + return spdval1 * CV.KPH_TO_MS + elif tsgn1 == 36 or frogpilot_variables.force_mph_dashboard: + return spdval1 * CV.MPH_TO_MS + else: + return 0 + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() @@ -196,6 +211,8 @@ class CarState(CarStateBase): self.cruise_decreased = self.pcm_acc_status == 10 self.cruise_increased = self.pcm_acc_status == 9 + fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp_cam, frogpilot_variables) + 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 @@ -292,6 +309,11 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): messages = [] + messages += [ + ("RSA1", 0), + ("RSA2", 0), + ] + if CP.carFingerprint != CAR.PRIUS_V: messages += [ ("LKAS_HUD", 1), diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index c891080..993b6ba 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -207,7 +207,7 @@ class FrogPilotPlanner: # Pfeiferj's Speed Limit Controller if frogpilot_toggles.speed_limit_controller: - SpeedLimitController.update(frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles) + SpeedLimitController.update(frogpilotCarState.dashboardSpeedLimit, frogpilotNavigation.navigationSpeedLimit, 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 0b1fa91..55a49e9 100644 --- a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -25,11 +25,13 @@ 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.nav_speed_limit = 0 # m/s self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit") - def update(self, navigationSpeedLimit, v_ego, frogpilot_toggles): + def update(self, dashboardSpeedLimit, navigationSpeedLimit, v_ego, frogpilot_toggles): + self.car_speed_limit = dashboardSpeedLimit self.write_map_state(v_ego) self.nav_speed_limit = navigationSpeedLimit @@ -70,7 +72,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 = [limit for limit in limits if limit is not None and limit > 1] if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits: @@ -79,6 +81,7 @@ class SpeedLimitController: return float(min(filtered_limits)) speed_limits = { + "Dashboard": self.car_speed_limit, "Offline Maps": self.map_speed_limit, "Navigation": self.nav_speed_limit, }