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

fordcan cleanup

Browse Source 
Co-authored-by: Cameron Clough <cameronjclough@gmail.com>
pull/27089/head
Shane Smiskol 3 years ago
parent dd2e44232c
commit 9170f75819
2 changed files with 28 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      selfdrive/car/ford/carcontroller.py
  2. 21
      selfdrive/car/ford/fordcan.py

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

@ -46,15 +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(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.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(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.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(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True))
### lateral control ###
@ -84,8 +84,8 @@ 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,
can_sends.append(fordcan.create_lka_msg(self.packer, 0, 0))
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision,
0, path_angle, 0, 0))
@ -94,11 +94,11 @@ class CarController:
# 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(fordcan.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(fordcan.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

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

@ -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, angle_deg: float, curvature: float):
"""
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.
@ -25,7 +25,7 @@ def create_lka_command(packer, angle_deg: float, curvature: float):
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_rate: float, curvature: float):
"""
Creates a CAN message for the Ford TJA/LCA Command.
@ -34,9 +34,9 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
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
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.
@ -50,7 +50,8 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
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]
"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
@ -61,7 +62,7 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
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.
@ -113,7 +114,7 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
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.
@ -148,7 +149,7 @@ 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.

Write Preview
Loading…
Cancel
Save