diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 8c391bb276..c216a6454f 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -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 diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 373ce096c6..bf97018261 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -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. @@ -48,20 +48,21 @@ 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] - "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. @@ -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.