From c0c461862c2af847c34bd9d300a753bfdb1a33e8 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Tue, 20 Feb 2024 20:50:17 +0000 Subject: [PATCH 1/9] Update carcontroller.py --- cereal | 2 +- opendbc | 2 +- panda | 2 +- selfdrive/car/ford/carcontroller.py | 10 ++++++++-- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/cereal b/cereal index e86a48cf41..f87b5b8395 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e86a48cf413c48e8d5a0d247f3b64fdcb800096f +Subproject commit f87b5b839582881c4c3aed07cc2c3c20bb03df8f diff --git a/opendbc b/opendbc index a49eea191e..c88be33a8d 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit a49eea191e4957fc149178c44ee418d3387194c3 +Subproject commit c88be33a8d2d81a7043e58025decce2da4e6bad0 diff --git a/panda b/panda index 53ad150cdf..24ccef2efe 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 53ad150cdf0d1f7a61f5ac6da7a5bd3b0d6fec3e +Subproject commit 24ccef2efe7feac3ddb81dd7e697c0614e6fd541 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 261119c485..63ac9dc4e5 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,7 +3,7 @@ from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams +from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, FordFlagsSP from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState @@ -34,6 +34,9 @@ class CarController: self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False + self.path_angle = 0. + self.path_offset = 0. + self.curvature_rate = 0. def update(self, CC, CS, now_nanos): can_sends = [] @@ -73,7 +76,10 @@ class CarController: # TODO: extended mode mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) + if self.CP.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value: + can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, self.path_offset, self.path_angle, -apply_curvature, self.curvature_rate, counter)) + else: + can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) else: can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.)) From 4653f530d28c0889a6137198a7c2e672501c569f Mon Sep 17 00:00:00 2001 From: Isaac Date: Tue, 20 Feb 2024 22:30:23 -0500 Subject: [PATCH 2/9] Revert "Update carcontroller.py" This reverts commit c0c461862c2af847c34bd9d300a753bfdb1a33e8. --- cereal | 2 +- opendbc | 2 +- panda | 2 +- selfdrive/car/ford/carcontroller.py | 10 ++-------- 4 files changed, 5 insertions(+), 11 deletions(-) diff --git a/cereal b/cereal index f87b5b8395..e86a48cf41 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit f87b5b839582881c4c3aed07cc2c3c20bb03df8f +Subproject commit e86a48cf413c48e8d5a0d247f3b64fdcb800096f diff --git a/opendbc b/opendbc index c88be33a8d..a49eea191e 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit c88be33a8d2d81a7043e58025decce2da4e6bad0 +Subproject commit a49eea191e4957fc149178c44ee418d3387194c3 diff --git a/panda b/panda index 24ccef2efe..53ad150cdf 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 24ccef2efe7feac3ddb81dd7e697c0614e6fd541 +Subproject commit 53ad150cdf0d1f7a61f5ac6da7a5bd3b0d6fec3e diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 63ac9dc4e5..261119c485 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,7 +3,7 @@ from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, FordFlagsSP +from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState @@ -34,9 +34,6 @@ class CarController: self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False - self.path_angle = 0. - self.path_offset = 0. - self.curvature_rate = 0. def update(self, CC, CS, now_nanos): can_sends = [] @@ -76,10 +73,7 @@ class CarController: # TODO: extended mode mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF - if self.CP.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value: - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, self.path_offset, self.path_angle, -apply_curvature, self.curvature_rate, counter)) - else: - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) + can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) else: can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.)) From bda7a60f867e32594654777b4a291c5cc721056c Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Tue, 20 Feb 2024 20:50:17 +0000 Subject: [PATCH 3/9] Updating Car Controller --- selfdrive/car/ford/carcontroller.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 261119c485..63ac9dc4e5 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,7 +3,7 @@ from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams +from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, FordFlagsSP from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState @@ -34,6 +34,9 @@ class CarController: self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False + self.path_angle = 0. + self.path_offset = 0. + self.curvature_rate = 0. def update(self, CC, CS, now_nanos): can_sends = [] @@ -73,7 +76,10 @@ class CarController: # TODO: extended mode mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) + if self.CP.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value: + can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, self.path_offset, self.path_angle, -apply_curvature, self.curvature_rate, counter)) + else: + can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) else: can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.)) From f5374f6e892f329f07250e3a66552fd7bc087c5b Mon Sep 17 00:00:00 2001 From: Isaac Date: Tue, 20 Feb 2024 16:08:39 -0500 Subject: [PATCH 4/9] Cherry pick --- selfdrive/car/ford/interface.py | 9 ++++++++- selfdrive/car/ford/values.py | 3 ++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index fadae1d93c..9978b1ab6b 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -3,7 +3,8 @@ from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import get_safety_config, create_mads_event from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu, BUTTON_STATES +from openpilot.common.params import Params +from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu, BUTTON_STATES, FordFlagsSP from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -45,6 +46,12 @@ class CarInterface(CarInterfaceBase): if candidate in CANFD_CAR: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD + if ret.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL: + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_ENHANCED_LAT_CONTROL + + if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678") + ret.spFlags |= FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value + if candidate == CAR.BRONCO_SPORT_MK1: ret.wheelbase = 2.67 ret.steerRatio = 17.7 diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 387d3e6a50..84cdb56097 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -39,7 +39,6 @@ class CarControllerParams: def __init__(self, CP): pass - BUTTON_STATES = { "accelCruise": False, "decelCruise": False, @@ -134,3 +133,5 @@ FW_QUERY_CONFIG = FwQueryConfig( (Ecu.shiftByWire, 0x732, None), ], ) +class FordFlagsSP(IntFlag): + SP_ENHANCED_LAT_CONTROL = 1 \ No newline at end of file From 9d917718223a71b3f065634b7310cdabaaa72be1 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Tue, 20 Feb 2024 22:39:49 +0000 Subject: [PATCH 5/9] Update values.py --- selfdrive/car/ford/values.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 84cdb56097..24d99f9a54 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,6 +1,6 @@ from collections import defaultdict from dataclasses import dataclass -from enum import Enum, StrEnum +from enum import Enum, StrEnum, IntFlag from typing import Dict, List, Union from cereal import car @@ -134,4 +134,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) class FordFlagsSP(IntFlag): - SP_ENHANCED_LAT_CONTROL = 1 \ No newline at end of file + SP_ENHANCED_LAT_CONTROL = 1 From 6e2ba55a99d69f9e9f58809c0dd02cd73f1db3c7 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Tue, 20 Feb 2024 23:01:28 +0000 Subject: [PATCH 6/9] Update interface.py --- selfdrive/car/ford/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 9978b1ab6b..6cfe188e77 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -49,7 +49,7 @@ class CarInterface(CarInterfaceBase): if ret.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_ENHANCED_LAT_CONTROL - if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678") + if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678"): ret.spFlags |= FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value if candidate == CAR.BRONCO_SPORT_MK1: From a2b0275e37db16d9b321673888a3a8e30687d1bc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 16 May 2024 02:07:31 -0400 Subject: [PATCH 7/9] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 221f016f07..f86b0950cc 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 221f016f073c7ba824e7f3dd427046cafe2fca62 +Subproject commit f86b0950cc257cbde1ed360ee2872d732937fd7a From b01d6d423a7b4d6c08d50ef369ea292347a4c810 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 16 May 2024 02:09:32 -0400 Subject: [PATCH 8/9] update flag --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index f86b0950cc..afa4a376bb 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit f86b0950cc257cbde1ed360ee2872d732937fd7a +Subproject commit afa4a376bb4f145d7829037547ece3b8ceff3482 From 17b7f0bdba17ab2270675892c1b94ef4c08bcb7d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 16 May 2024 02:10:40 -0400 Subject: [PATCH 9/9] before flag is set --- selfdrive/car/ford/interface.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index f8cc581101..9142fc6cc6 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -32,6 +32,9 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kpV = [0.5] ret.longitudinalTuning.kiV = [0.] + if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678"): + ret.spFlags |= FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value + CAN = CanBus(fingerprint=fingerprint) cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)] if CAN.main >= 4: @@ -49,9 +52,6 @@ class CarInterface(CarInterfaceBase): if ret.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_ENHANCED_LAT_CONTROL - if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678"): - ret.spFlags |= FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value - # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs: