diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 592d8586ca..f18014601c 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,7 +3,7 @@ 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.values import CarControllerParams +from selfdrive.car.ford.values import CANBUS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -16,9 +16,9 @@ def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo): apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) # absolute limit (LatCtlPath_An_Actl) - apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO - apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240) - apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO + apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO + apply_path_angle = clip(apply_path_angle, -0.5, 0.5235) + apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO return apply_angle @@ -47,40 +47,46 @@ class CarController: ### acc buttons ### if CC.cruiseControl.cancel: can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) - elif CC.cruiseControl.resume: + can_sends.append(fordcan.create_button_command(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)) - - # if stock lane centering is active or in standby, toggle it off + can_sends.append(fordcan.create_button_command(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 - if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 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)) ### lateral control ### if CC.latActive: + lca_rq = 1 apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) else: - apply_angle = CS.out.steeringAngleDeg + lca_rq = 0 + apply_angle = 0. # send steering commands at 20Hz if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: - lca_rq = 1 if CC.latActive else 0 - # use LatCtlPath_An_Actl to actuate steering - # path angle is the car wheel angle, not the steering wheel angle - path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO - - # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately - # TODO: try slower ramp speed when driver torque detected - ramp_type = 3 + path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO + + # set slower ramp type when small steering angle change + # 0=Slow, 1=Medium, 2=Fast, 3=Immediately + steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg) + if steer_change < 2.0: + ramp_type = 0 + elif steer_change < 4.0: + ramp_type = 1 + elif steer_change < 6.0: + ramp_type = 2 + else: + ramp_type = 3 precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) - offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094) - self.apply_angle_last = apply_angle - can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0)) + 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, offset_roll_compensation_curvature)) + 0, path_angle, 0, 0)) ### ui ### @@ -99,7 +105,7 @@ class CarController: self.steer_alert_last = steer_alert new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = apply_angle + new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index a7ea19effc..2276b1208a 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): ret = car.CarState.new_message() # car speed - ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS + ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 @@ -85,8 +85,6 @@ class CarState(CarStateBase): # Stock values from IPMA so that we can retain some stock functionality self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] - # Use stock sensor values - self.yaw_data = cp.vl["Yaw_Data_FD1"] return ret @@ -94,7 +92,7 @@ class CarState(CarStateBase): def get_can_parser(CP): signals = [ # sig_name, sig_address - ("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) + ("Veh_V_ActlBrk", "BrakeSysFeatures"), # ABS vehicle speed (kph) ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped ("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status @@ -156,7 +154,7 @@ class CarState(CarStateBase): checks = [ # sig_address, frequency - ("EngVehicleSpThrottle2", 50), + ("BrakeSysFeatures", 50), ("Yaw_Data_FD1", 100), ("DesiredTorqBrk", 50), ("EngVehicleSpThrottle", 100), diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index b42561df21..373ce096c6 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -8,8 +8,7 @@ def create_lka_command(packer, angle_deg: float, curvature: float): """ Creates a CAN message for the Ford LKAS 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. Frequency is 20Hz. """ @@ -30,12 +29,20 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path """ 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. - 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. + 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 + 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. + + 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. """ @@ -47,7 +54,7 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path "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.4995|0.5240] radians + "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 } @@ -108,8 +115,8 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo def create_acc_ui_command(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. @@ -141,7 +148,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 = CANBUS.camera): +def create_button_command(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. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 7d4c9eb94c..4943db076f 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -5,8 +5,7 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter from selfdrive.car.interfaces import CarInterfaceBase - -EventName = car.CarEvent.EventName +CarParams = car.CarParams class CarInterface(CarInterfaceBase): @@ -19,10 +18,10 @@ class CarInterface(CarInterfaceBase): ret.carName = "ford" ret.dashcamOnly = True - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] + ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] # Angle-based steering - ret.steerControlType = car.CarParams.SteerControlType.angle + ret.steerControlType = CarParams.SteerControlType.angle ret.steerActuatorDelay = 0.4 ret.steerLimitTimer = 1.0 tire_stiffness_factor = 1.0 @@ -43,7 +42,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 1350 + STD_CARGO_KG else: - raise ValueError(f"Unsupported car: ${candidate}") + raise ValueError(f"Unsupported car: {candidate}") # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 5820b5c9fd..7b3140fbbf 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,4 +1,4 @@ -from collections import namedtuple +from collections import defaultdict, namedtuple from dataclasses import dataclass from enum import Enum from typing import Dict, List, Union @@ -22,19 +22,17 @@ class CarControllerParams: LKAS_UI_STEP = 100 # Message: ACCDATA_3 ACC_UI_STEP = 5 + # Message: Steering_Data_FD1, but send twice as fast + BUTTONS_STEP = 10 / 2 - STEER_RATIO = 2.75 - STEER_DRIVER_ALLOWANCE = 0.8 + LKAS_STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians + # TODO: remove this once we understand how the EPS calculates the steering angle better + STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold in Nm RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) -class RADAR: - DELPHI_ESR = 'ford_fusion_2018_adas' - DELPHI_MRR = 'FORD_CADS' - - class CANBUS: main = 0 radar = 1 @@ -47,6 +45,14 @@ class CAR: FOCUS_MK4 = "FORD FOCUS 4TH GEN" +class RADAR: + DELPHI_ESR = 'ford_fusion_2018_adas' + DELPHI_MRR = 'FORD_CADS' + + +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR)) + + @dataclass class FordCarInfo(CarInfo): package: str = "Co-Pilot360 Assist+" @@ -143,10 +149,3 @@ FW_VERSIONS = { ], }, } - - -DBC = { - CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), - CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), - CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), -}