diff --git a/panda b/panda index 67c63b3cf9..02fa8fbe8f 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 67c63b3cf9b2c2f537d90ef8d6ce0992e01457b6 +Subproject commit 02fa8fbe8f93dff077ba69f85eac305fa7444989 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 47082fb56f..48fbd06b4b 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker from openpilot.common.numpy_fast import clip from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan -from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags +from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags, FordFlagsSP from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX @@ -36,6 +36,9 @@ class CarController(CarControllerBase): self.lkas_enabled_last = False self.steer_alert_last = False self.lead_distance_bars_last = None + self.path_angle = 0. + self.path_offset = 0. + self.curvature_rate = 0. def update(self, CC, CS, now_nanos): can_sends = [] @@ -75,7 +78,10 @@ class CarController(CarControllerBase): # TODO: extended mode mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - 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.)) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index bce7dfd2d6..f24d819d2a 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 create_button_events, get_safety_config, create_mads_event from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import Ecu, FordFlags, BUTTON_STATES +from openpilot.common.params import Params +from openpilot.selfdrive.car.ford.values import Ecu, FordFlags, BUTTON_STATES, FordFlagsSP from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -30,6 +31,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: @@ -44,6 +48,9 @@ class CarInterface(CarInterfaceBase): if ret.flags & FordFlags.CANFD: 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 + ret.longitudinalTuning.kpBP = [0.] ret.longitudinalTuning.kpV = [0.5] ret.longitudinalTuning.kiV = [0.] diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index edfee3ea1b..64bce98da6 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -46,6 +46,10 @@ class FordFlags(IntFlag): CANFD = 1 +class FordFlagsSP(IntFlag): + SP_ENHANCED_LAT_CONTROL = 1 + + BUTTON_STATES = { "accelCruise": False, "decelCruise": False,