From 39b2dcdc0f460bbff1c7486189dd4116af19acb3 Mon Sep 17 00:00:00 2001 From: Isaac <48419673+coffee-cake-isaac@users.noreply.github.com> Date: Sun, 17 Dec 2023 12:25:58 -0500 Subject: [PATCH 1/7] Added speed sign recognition on CAN-FD Ford --- selfdrive/car/ford/carstate.py | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index d6265fd657..5c06da9680 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -24,6 +24,7 @@ class CarState(CarStateBase): self.prev_lkas_enabled = None self.buttonStates = BUTTON_STATES.copy() self.buttonStatesPrev = BUTTON_STATES.copy() + self._vLimit = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -75,6 +76,11 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0 ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3 ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2) + + # SpeedLimit + self.update_traffic_signals(self.CP, cp, cp_cam) + ret.cruiseState.speedLimit = self._vLimit + if not self.CP.openpilotLongitudinalControl: ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1 @@ -127,6 +133,21 @@ class CarState(CarStateBase): return ret + + def update_traffic_signals(self, CP, cp, cp_cam): + if CP.carFingerprint in CANFD_CAR: + # we'll start with CANFD - not sure if and how to check if this message is in non CANFD + origVLimit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"] + origVLimitUnit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"] + + # convert to m/s from whatever came in + speed_factor = CV.MPH_TO_MS if origVLimitUnit == 2 else CV.KPH_TO_MS if origVLimitUnit == 1 else 0 + + vLimit = origVLimit if origVLimit not in (0, 255) else 0 + + # speed in m/s + self._vLimit = vLimit * speed_factor + @staticmethod def get_can_parser(CP): messages = [ @@ -161,6 +182,11 @@ class CarState(CarStateBase): ("BCM_Lamp_Stat_FD1", 1), ] + if CP.carFingerprint in CANFD_CAR: + messages += [ + ("Traffic_RecognitnData", 1), + ] + if CP.enableBsm and CP.carFingerprint not in CANFD_CAR: messages += [ ("Side_Detect_L_Stat", 5), From b6f1a5c42995c2187ff00b6cee0583aaa96eb729 Mon Sep 17 00:00:00 2001 From: Isaac <48419673+coffee-cake-isaac@users.noreply.github.com> Date: Sun, 17 Dec 2023 12:43:57 -0500 Subject: [PATCH 2/7] Fixed naming --- selfdrive/car/ford/carstate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 5c06da9680..cbf0270f94 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -24,7 +24,7 @@ class CarState(CarStateBase): self.prev_lkas_enabled = None self.buttonStates = BUTTON_STATES.copy() self.buttonStatesPrev = BUTTON_STATES.copy() - self._vLimit = 0 + self.prev_vLimit = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -79,7 +79,7 @@ class CarState(CarStateBase): # SpeedLimit self.update_traffic_signals(self.CP, cp, cp_cam) - ret.cruiseState.speedLimit = self._vLimit + ret.cruiseState.speedLimit = self.prev_vLimit if not self.CP.openpilotLongitudinalControl: ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1 @@ -146,7 +146,7 @@ class CarState(CarStateBase): vLimit = origVLimit if origVLimit not in (0, 255) else 0 # speed in m/s - self._vLimit = vLimit * speed_factor + self.prev_vLimit = vLimit * speed_factor @staticmethod def get_can_parser(CP): From c66f3492c5835eac733a964949542e0d5b180cec Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 19 Dec 2023 17:35:39 -0500 Subject: [PATCH 3/7] cleanup --- selfdrive/car/ford/carstate.py | 30 ++++++++++-------------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index cbf0270f94..9fd2a6992e 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -77,9 +77,8 @@ class CarState(CarStateBase): ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3 ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2) - # SpeedLimit - self.update_traffic_signals(self.CP, cp, cp_cam) - ret.cruiseState.speedLimit = self.prev_vLimit + if self.CP.carFingerprint in CANFD_CAR: + ret.cruiseState.speedLimit = self.update_traffic_signals(cp_cam) if not self.CP.openpilotLongitudinalControl: ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1 @@ -133,20 +132,15 @@ class CarState(CarStateBase): return ret + def update_traffic_signals(self, cp_cam): + # TODO: Check if CAN platforms have the same signals + if self.CP.carFingerprint in CANFD_CAR: + v_limit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"] + v_limit_unit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"] - def update_traffic_signals(self, CP, cp, cp_cam): - if CP.carFingerprint in CANFD_CAR: - # we'll start with CANFD - not sure if and how to check if this message is in non CANFD - origVLimit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"] - origVLimitUnit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"] + speed_factor = CV.MPH_TO_MS if v_limit_unit == 2 else CV.KPH_TO_MS if v_limit_unit == 1 else 0 - # convert to m/s from whatever came in - speed_factor = CV.MPH_TO_MS if origVLimitUnit == 2 else CV.KPH_TO_MS if origVLimitUnit == 1 else 0 - - vLimit = origVLimit if origVLimit not in (0, 255) else 0 - - # speed in m/s - self.prev_vLimit = vLimit * speed_factor + return v_limit * speed_factor if v_limit not in (0, 255) else 0 @staticmethod def get_can_parser(CP): @@ -175,6 +169,7 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: messages += [ ("Gear_Shift_by_Wire_FD1", 10), + ("Traffic_RecognitnData", 1), ] elif CP.transmissionType == TransmissionType.manual: messages += [ @@ -182,11 +177,6 @@ class CarState(CarStateBase): ("BCM_Lamp_Stat_FD1", 1), ] - if CP.carFingerprint in CANFD_CAR: - messages += [ - ("Traffic_RecognitnData", 1), - ] - if CP.enableBsm and CP.carFingerprint not in CANFD_CAR: messages += [ ("Side_Detect_L_Stat", 5), From 1cf45623f144e26255240b9c2c20cccb95f0b2b0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 19 Dec 2023 17:36:45 -0500 Subject: [PATCH 4/7] more cleanup --- selfdrive/car/ford/carstate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 9fd2a6992e..c85c92f6b4 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -24,7 +24,7 @@ class CarState(CarStateBase): self.prev_lkas_enabled = None self.buttonStates = BUTTON_STATES.copy() self.buttonStatesPrev = BUTTON_STATES.copy() - self.prev_vLimit = 0 + self.v_limit = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -135,12 +135,12 @@ class CarState(CarStateBase): def update_traffic_signals(self, cp_cam): # TODO: Check if CAN platforms have the same signals if self.CP.carFingerprint in CANFD_CAR: - v_limit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"] + self.v_limit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"] v_limit_unit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"] speed_factor = CV.MPH_TO_MS if v_limit_unit == 2 else CV.KPH_TO_MS if v_limit_unit == 1 else 0 - return v_limit * speed_factor if v_limit not in (0, 255) else 0 + return self.v_limit * speed_factor if self.v_limit not in (0, 255) else 0 @staticmethod def get_can_parser(CP): From 3aa30a5c5993910f6b835c32c1de8e0d0c6fb8ca Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 19 Dec 2023 17:51:43 -0500 Subject: [PATCH 5/7] Update CHANGELOGS.md --- CHANGELOGS.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 981a98717c..cfa9d96ccc 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -1,5 +1,7 @@ sunnypilot - 0.9.6.1 (2023-xx-xx) ======================== +* NEW❗: Ford CAN-FD longitudinal + * NEW❗: Parse speed limit sign recognition from camera for certain supported platforms * UPDATED: Dynamic Experimental Control (DEC) * Synced with dragonpilot-community/dragonpilot:beta3 commit dd4c663 * UPDATED: Vision-based Turn Speed Control (V-TSC) implementation From e34ec127d1846bd8652e7f8942607b9a519ba815 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Wed, 20 Dec 2023 13:54:44 +0000 Subject: [PATCH 6/7] Update carstate.py --- selfdrive/car/ford/carstate.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index c85c92f6b4..acc8639adc 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -169,7 +169,6 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: messages += [ ("Gear_Shift_by_Wire_FD1", 10), - ("Traffic_RecognitnData", 1), ] elif CP.transmissionType == TransmissionType.manual: messages += [ @@ -195,6 +194,11 @@ class CarState(CarStateBase): ("IPMA_Data", 1), ] + if CP.carFingerprint in CANFD_CAR: + messages += [ + ("Traffic_RecognitnData", 1), + ] + if CP.enableBsm and CP.carFingerprint in CANFD_CAR: messages += [ ("Side_Detect_L_Stat", 5), From 79252b6b5ae30ec13514ec8cb68fa5c377ec7a95 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 16 May 2024 02:03:17 -0400 Subject: [PATCH 7/7] Update CHANGELOGS.md --- CHANGELOGS.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index b8fa8dda07..3cfd274453 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -24,6 +24,8 @@ sunnypilot - 0.9.7.0 (2024-05-xx) * comma Prime support * Personal Mapbox/Amap/Google Maps token support * Instructions on how to set up your iOS Siri Shortcuts: https://routinehub.co/shortcut/17677/ +* NEW❗: Ford CAN-FD longitudinal + * NEW❗: Parse speed limit sign recognition from camera for certain supported platforms * RE-ENABLED: Map-based Turn Speed Control (M-TSC) for supported platforms * openpilot Longitudianl Control available cars * Custom Stock Longitudinal Control available cars @@ -110,8 +112,6 @@ sunnypilot - 0.9.6.1 (2024-02-27) sunnypilot - 0.9.5.3 (2023-12-24) ======================== -* NEW❗: Ford CAN-FD longitudinal - * NEW❗: Parse speed limit sign recognition from camera for certain supported platforms * UPDATED: Dynamic Experimental Control (DEC) * Synced with dragonpilot-community/dragonpilot:lp-dp-beta2 commit 578d38b * UPDATED: Driving Model Selector v2