mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 12:02:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master-priv-ford-lat' into master-dev-c3-new

Browse Source 
# Conflicts:
#	panda
#	selfdrive/car/ford/interface.py
This commit is contained in:
Jason Wen
2024-05-16 11:01:54 -04:00
parent bab98e9c56 17b7f0bdba
commit 54e97eec07
4 changed files with 21 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda
+1 -1
Submodule panda updated: 67c63b3cf9...02fa8fbe8f
selfdrive/car/ford/carcontroller.py
+8 -2
View File
@@ -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.))
selfdrive/car/ford/interface.py
+8 -1
View File
@@ -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.]
selfdrive/car/ford/values.py
+4
View File
@@ -46,6 +46,10 @@ class FordFlags(IntFlag):
CANFD = 1
class FordFlagsSP(IntFlag):
SP_ENHANCED_LAT_CONTROL = 1
BUTTON_STATES = {
"accelCruise": False,
"decelCruise": False,