mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-05 00:05:59 +08:00
Ford cleanup (#27090)
* fordcan cleanup Co-authored-by: Cameron Clough <cameronjclough@gmail.com> * no fordcan Co-authored-by: Cameron Clough <cameronjclough@gmail.com> * more Co-authored-by: Cameron Clough <cameronjclough@gmail.com> * these weren't even used in CS, follow rest of the brands * we won't use this * rename to more standard powertrain can bus * no arguments for lka anymore * Revert "rename to more standard powertrain can bus" This reverts commit 0bc3f79f9bad2218c48a350bbf5ec6f726e0a028. Co-authored-by: Shane Smiskol <shane@smiskol.com>
This commit is contained in:
Cameron Clough
@@ -2,7 +2,7 @@ import math
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car.ford import fordcan
|
||||
from selfdrive.car.ford.fordcan import create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg
|
||||
from selfdrive.car.ford.values import CANBUS, CarControllerParams
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
@@ -46,16 +46,15 @@ class CarController:
|
||||
|
||||
### acc buttons ###
|
||||
if CC.cruiseControl.cancel:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True))
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main))
|
||||
can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True))
|
||||
can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main))
|
||||
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True))
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
|
||||
can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True))
|
||||
can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
|
||||
# if stock lane centering isn't off, send a button press to toggle it off
|
||||
# the stock system checks for steering pressed, and eventually disengages cruise control
|
||||
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True))
|
||||
|
||||
can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True))
|
||||
|
||||
### lateral control ###
|
||||
if CC.latActive:
|
||||
@@ -84,21 +83,19 @@ class CarController:
|
||||
precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
|
||||
|
||||
self.apply_angle_last = apply_angle
|
||||
can_sends.append(fordcan.create_lka_command(self.packer, 0, 0))
|
||||
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
|
||||
0, path_angle, 0, 0))
|
||||
|
||||
can_sends.append(create_lka_msg(self.packer))
|
||||
can_sends.append(create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision, 0, path_angle, 0, 0))
|
||||
|
||||
### ui ###
|
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
|
||||
|
||||
# send lkas ui command at 1Hz or if ui state changes
|
||||
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
|
||||
can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
|
||||
|
||||
# send acc ui command at 20Hz or if ui state changes
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values))
|
||||
can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values))
|
||||
|
||||
self.main_on_last = main_on
|
||||
self.lkas_enabled_last = CC.latActive
|
||||
|
||||
@@ -118,7 +118,7 @@ class CarState(CarStateBase):
|
||||
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
|
||||
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
|
||||
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver
|
||||
("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons
|
||||
("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthrough the remaining buttons
|
||||
("WiprFront_D_Stat", "Steering_Data_FD1"),
|
||||
("LghtAmb_D_Sns", "Steering_Data_FD1"),
|
||||
("AccButtnGapDecPress", "Steering_Data_FD1"),
|
||||
|
||||
@@ -4,9 +4,9 @@ from selfdrive.car.ford.values import CANBUS
|
||||
HUDControl = car.CarControl.HUDControl
|
||||
|
||||
|
||||
def create_lka_command(packer, angle_deg: float, curvature: float):
|
||||
def create_lka_msg(packer):
|
||||
"""
|
||||
Creates a CAN message for the Ford LKAS Command.
|
||||
Creates a CAN message for the Ford LKA Command.
|
||||
|
||||
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
|
||||
|
||||
@@ -16,52 +16,54 @@ def create_lka_command(packer, angle_deg: float, curvature: float):
|
||||
values = {
|
||||
"LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3]
|
||||
"LkaActvStats_D2_Req": 0, # action [0|7]
|
||||
"LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees
|
||||
"LaRefAng_No_Req": 0, # angle [-102.4|102.3] degrees
|
||||
"LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick
|
||||
"LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter
|
||||
"LaCurvature_No_Calc": 0, # curvature [-0.01024|0.01023] 1/meter
|
||||
"LdwActvStats_D_Req": 0, # LDW status [0|7]
|
||||
"LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength
|
||||
}
|
||||
return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float):
|
||||
def create_lat_ctl_msg(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float,
|
||||
curvature: float, curvature_rate: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford TJA/LCA Command.
|
||||
|
||||
This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam
|
||||
assist and highway driving. It is not subject to the PSCM lockout.
|
||||
This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam assist and highway
|
||||
driving. It is not subject to the PSCM lockout.
|
||||
|
||||
Ford lane centering command uses a third order polynomial to describe the road centerline. The
|
||||
polynomial is defined by the following coefficients:
|
||||
c0: lateral offset between the vehicle and the centerline
|
||||
c1: heading angle between the vehicle and the centerline
|
||||
c2: curvature of the centerline
|
||||
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
|
||||
by the following coefficients:
|
||||
c0: lateral offset between the vehicle and the centerline (positive is right)
|
||||
c1: heading angle between the vehicle and the centerline (positive is right)
|
||||
c2: curvature of the centerline (positive is left)
|
||||
c3: rate of change of curvature of the centerline
|
||||
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and
|
||||
speed, the steering angle cannot be easily controlled.
|
||||
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
|
||||
angle cannot be easily controlled.
|
||||
|
||||
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed.
|
||||
This can be done using tools such as Forscan.
|
||||
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
|
||||
using tools such as Forscan.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
|
||||
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
|
||||
# 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
|
||||
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
||||
}
|
||||
return packer.make_can_msg("LateralMotionControl", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict):
|
||||
def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status.
|
||||
|
||||
@@ -107,16 +109,15 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
|
||||
|
||||
values = {
|
||||
**stock_values,
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
}
|
||||
return packer.make_can_msg("IPMA_Data", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
|
||||
def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
|
||||
assist status.
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status.
|
||||
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
@@ -148,7 +149,8 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, sto
|
||||
return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus: int = CANBUS.camera):
|
||||
def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tja_toggle=False,
|
||||
bus: int = CANBUS.camera):
|
||||
"""
|
||||
Creates a CAN message for the Ford SCCM buttons/switches.
|
||||
|
||||
|
||||
@@ -2,10 +2,11 @@
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import STD_CARGO_KG, get_safety_config
|
||||
from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter
|
||||
from selfdrive.car.ford.values import CAR, Ecu
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
CarParams = car.CarParams
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@@ -13,10 +14,10 @@ class CarInterface(CarInterfaceBase):
|
||||
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
|
||||
ret.carName = "ford"
|
||||
ret.dashcamOnly = True
|
||||
ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)]
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
|
||||
# Angle-based steering
|
||||
ret.steerControlType = CarParams.SteerControlType.angle
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.4
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
|
||||
@@ -9,9 +9,6 @@ from selfdrive.car.docs_definitions import CarInfo, Harness
|
||||
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user