sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ford cleanup (#27090)

Browse Source 
* 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 0bc3f79f9b.

Co-authored-by: Shane Smiskol <shane@smiskol.com>
old-commit-hash: 3cd60a4724
beeps
Cameron Clough 2 years ago committed by GitHub
parent 34e681f8d6
commit 71f40532f0
5 changed files with 50 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 23
      selfdrive/car/ford/carcontroller.py
  2. 2
      selfdrive/car/ford/carstate.py
  3. 46
      selfdrive/car/ford/fordcan.py
  4. 9
      selfdrive/car/ford/interface.py
  5. 3
      selfdrive/car/ford/values.py

23
selfdrive/car/ford/carcontroller.py
Unescape View File

@ -2,7 +2,7 @@ import math
from cereal import car from cereal import car
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker 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 from selfdrive.car.ford.values import CANBUS, CarControllerParams
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -46,16 +46,15 @@ class CarController:
### acc buttons ### ### acc buttons ###
if CC.cruiseControl.cancel: if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) can_sends.append(create_button_msg(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, bus=CANBUS.main))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: 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(create_button_msg(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, bus=CANBUS.main))
# if stock lane centering isn't off, send a button press to toggle it off # 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 # 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: 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 ### ### lateral control ###
if CC.latActive: if CC.latActive:
@ -84,21 +83,19 @@ class CarController:
precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
self.apply_angle_last = apply_angle self.apply_angle_last = apply_angle
can_sends.append(fordcan.create_lka_command(self.packer, 0, 0)) can_sends.append(create_lka_msg(self.packer))
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, can_sends.append(create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision, 0, path_angle, 0, 0))
0, path_angle, 0, 0))
### ui ### ### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) 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 # send lkas ui command at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: 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 # send acc ui command at 20Hz or if ui state changes
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: 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.main_on_last = main_on
self.lkas_enabled_last = CC.latActive self.lkas_enabled_last = CC.latActive

2
selfdrive/car/ford/carstate.py
Unescape View File

@ -118,7 +118,7 @@ class CarState(CarStateBase):
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver ("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"), ("WiprFront_D_Stat", "Steering_Data_FD1"),
("LghtAmb_D_Sns", "Steering_Data_FD1"), ("LghtAmb_D_Sns", "Steering_Data_FD1"),
("AccButtnGapDecPress", "Steering_Data_FD1"), ("AccButtnGapDecPress", "Steering_Data_FD1"),

46
selfdrive/car/ford/fordcan.py
Unescape View File

@ -4,9 +4,9 @@ from selfdrive.car.ford.values import CANBUS
HUDControl = car.CarControl.HUDControl 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. This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
@ -16,33 +16,34 @@ def create_lka_command(packer, angle_deg: float, curvature: float):
values = { values = {
"LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3]
"LkaActvStats_D2_Req": 0, # action [0|7] "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 "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] "LdwActvStats_D_Req": 0, # LDW status [0|7]
"LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength
} }
return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values) 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. Creates a CAN message for the Ford TJA/LCA Command.
This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam assist and highway
assist and highway driving. It is not subject to the PSCM lockout. driving. It is not subject to the PSCM lockout.
Ford lane centering command uses a third order polynomial to describe the road centerline. The Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
polynomial is defined by the following coefficients: by the following coefficients:
c0: lateral offset between the vehicle and the centerline c0: lateral offset between the vehicle and the centerline (positive is right)
c1: heading angle between the vehicle and the centerline c1: heading angle between the vehicle and the centerline (positive is right)
c2: curvature of the centerline c2: curvature of the centerline (positive is left)
c3: rate of change of curvature of the centerline 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 As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
speed, the steering angle cannot be easily controlled. angle cannot be easily controlled.
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
This can be done using tools such as Forscan. using tools such as Forscan.
Frequency is 20Hz. Frequency is 20Hz.
""" """
@ -50,7 +51,8 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
values = { values = {
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter "LatCtlRng_L_Max": 0, # Unknown [0|126] meter
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] "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] "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] "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] "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 "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
@ -61,7 +63,7 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) 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. Creates a CAN message for the Ford IPC IPMA/LKAS status.
@ -113,10 +115,9 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
return packer.make_can_msg("IPMA_Data", CANBUS.main, values) 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 Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status.
assist status.
Stock functionality is maintained by passing through unmodified signals. 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) 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. Creates a CAN message for the Ford SCCM buttons/switches.

9
selfdrive/car/ford/interface.py
Unescape View File

@ -2,10 +2,11 @@
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config 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 from selfdrive.car.interfaces import CarInterfaceBase
CarParams = car.CarParams TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase): class CarInterface(CarInterfaceBase):
@ -13,10 +14,10 @@ class CarInterface(CarInterfaceBase):
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
ret.carName = "ford" ret.carName = "ford"
ret.dashcamOnly = True ret.dashcamOnly = True
ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
# Angle-based steering # Angle-based steering
ret.steerControlType = CarParams.SteerControlType.angle ret.steerControlType = car.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.4 ret.steerActuatorDelay = 0.4
ret.steerLimitTimer = 1.0 ret.steerLimitTimer = 1.0

3
selfdrive/car/ford/values.py
Unescape View File

@ -9,9 +9,6 @@ from selfdrive.car.docs_definitions import CarInfo, Harness
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu Ecu = car.CarParams.Ecu
TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])

Write Preview
Loading…
Cancel
Save