diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 5bdafa58b5..2a930e735e 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -2,9 +2,9 @@ from cereal import car from common.numpy_fast import clip from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_angle_limits -from selfdrive.car.ford.fordcan import create_acc_msg, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ - create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg -from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams +from selfdrive.car.ford.fordcan import CanBus, create_acc_msg, create_acc_ui_msg, create_button_msg, \ + create_lat_ctl_msg, create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg +from selfdrive.car.ford.values import CANFD_CARS, CarControllerParams LongCtrlState = car.CarControl.Actuators.LongControlState VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -27,6 +27,7 @@ class CarController: self.CP = CP self.VM = VM self.packer = CANPacker(dbc_name) + self.CAN = CanBus(CP) self.frame = 0 self.apply_curvature_last = 0 @@ -45,15 +46,15 @@ class CarController: ### acc buttons ### if CC.cruiseControl.cancel: - 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)) + can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True)) + can_sends.append(create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True)) elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: - 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)) + can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True)) + can_sends.append(create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True)) # 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(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) + can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True)) ### lateral control ### # send steer msg at 20Hz @@ -71,13 +72,13 @@ class CarController: # TODO: extended mode mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF - can_sends.append(create_lat_ctl2_msg(self.packer, mode, 0., 0., -apply_curvature, 0., counter)) + can_sends.append(create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) else: - can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + can_sends.append(create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.)) # send lka msg at 33Hz if (self.frame % CarControllerParams.LKA_STEP) == 0: - can_sends.append(create_lka_msg(self.packer)) + can_sends.append(create_lka_msg(self.packer, self.CAN)) ### longitudinal control ### # send acc msg at 50Hz @@ -89,16 +90,16 @@ class CarController: gas = CarControllerParams.INACTIVE_GAS stopping = CC.actuators.longControlState == LongCtrlState.stopping - can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, stopping)) + can_sends.append(create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping)) ### 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 msg at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: - can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + can_sends.append(create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) # send acc ui msg at 5Hz or if ui state changes if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(create_acc_ui_msg(self.packer, self.CP, main_on, CC.latActive, + can_sends.append(create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, CS.out.cruiseState.standstill, hud_control, CS.acc_tja_status_stock_values)) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 9be2c7637c..b768749325 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -3,7 +3,8 @@ from common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams +from selfdrive.car.ford.fordcan import CanBus +from selfdrive.car.ford.values import DBC, CarControllerParams GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -211,7 +212,7 @@ class CarState(CarStateBase): ("Side_Detect_R_Stat", 5), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).main) @staticmethod def get_cam_can_parser(CP): @@ -268,4 +269,4 @@ class CarState(CarStateBase): ("IPMA_Data", 1), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).camera) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 97a8c025d4..30a53597d6 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -1,9 +1,26 @@ from cereal import car -from selfdrive.car.ford.values import CANBUS +from selfdrive.car import CanBusBase HUDControl = car.CarControl.HUDControl +class CanBus(CanBusBase): + def __init__(self, CP=None, fingerprint=None) -> None: + super().__init__(CP, fingerprint) + + @property + def main(self) -> int: + return self.offset + + @property + def radar(self): + return self.offset + 1 + + @property + def camera(self): + return self.offset + 2 + + def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): curvature = (dat[2] << 3) | ((dat[3]) >> 5) curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5) @@ -17,7 +34,7 @@ def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): return 0xFF - (checksum & 0xFF) -def create_lka_msg(packer): +def create_lka_msg(packer, CAN: CanBus): """ Creates an empty CAN message for the Ford LKA Command. @@ -26,10 +43,10 @@ def create_lka_msg(packer): Frequency is 33Hz. """ - return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) + return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) -def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, +def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): """ Creates a CAN message for the Ford TJA/LCA Command. @@ -66,10 +83,10 @@ def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: "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) + return packer.make_can_msg("LateralMotionControl", CAN.main, values) -def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float, curvature: float, +def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float, curvature_rate: float, counter: int): """ Create a CAN message for the new Ford Lane Centering command. @@ -95,13 +112,13 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float } # calculate checksum - dat = packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)[2] + dat = packer.make_can_msg("LateralMotionControl2", 0, values)[2] values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) - return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) + return packer.make_can_msg("LateralMotionControl2", CAN.main, values) -def create_acc_msg(packer, long_active: bool, gas: float, accel: float, stopping: bool): +def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool): """ Creates a CAN message for the Ford ACC Command. @@ -122,10 +139,10 @@ def create_acc_msg(packer, long_active: bool, gas: float, accel: float, stopping "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active "AccStopStat_B_Rq": 1 if stopping else 0, } - return packer.make_can_msg("ACCDATA", CANBUS.main, values) + return packer.make_can_msg("ACCDATA", CAN.main, values) -def create_acc_ui_msg(packer, CP, main_on: bool, enabled: bool, standstill: bool, hud_control, +def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, standstill: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam @@ -197,10 +214,11 @@ def create_acc_ui_msg(packer, CP, main_on: bool, enabled: bool, standstill: bool "AccTGap_D_Dsply": 4, # Fixed time gap in UI }) - return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) + return packer.make_can_msg("ACCDATA_3", CAN.main, values) -def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): +def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control, + stock_values: dict): """ Creates a CAN message for the Ford IPC IPMA/LKAS status. @@ -263,11 +281,10 @@ def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, "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) + return packer.make_can_msg("IPMA_Data", CAN.main, values) -def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tja_toggle=False, - bus: int = CANBUS.camera): +def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False): """ 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 626853dd8f..fef7fcefc8 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -3,6 +3,7 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, get_safety_config +from selfdrive.car.ford.fordcan import CanBus from selfdrive.car.ford.values import CAR, Ecu from selfdrive.car.interfaces import CarInterfaceBase @@ -14,7 +15,6 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] # These cars are dashcam only for lack of test coverage. # Once a user confirms each car works and a test route is @@ -26,9 +26,15 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 + CAN = CanBus(fingerprint=fingerprint) + cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)] + if CAN.main >= 4: + cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput)) + ret.safetyConfigs = cfgs + ret.experimentalLongitudinalAvailable = True if experimental_long: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL ret.openpilotLongitudinalControl = True if candidate == CAR.BRONCO_SPORT_MK1: @@ -61,7 +67,7 @@ class CarInterface(CarInterfaceBase): # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] - if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0] or docs: + if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs: ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual @@ -69,7 +75,7 @@ class CarInterface(CarInterfaceBase): # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat # TODO: detect bsm in car_fw? - ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] + ret.enableBsm = 0x3A6 in fingerprint[CAN.main] and 0x3A7 in fingerprint[CAN.main] # LCA can steer down to zero ret.minSteerSpeed = 0. diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index ee4efb311d..e44730ca4f 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -3,7 +3,8 @@ from math import cos, sin from cereal import car from opendbc.can.parser import CANParser from common.conversions import Conversions as CV -from selfdrive.car.ford.values import CANBUS, DBC, RADAR +from selfdrive.car.ford.fordcan import CanBus +from selfdrive.car.ford.values import DBC, RADAR from selfdrive.car.interfaces import RadarInterfaceBase DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540)) @@ -12,16 +13,16 @@ DELPHI_MRR_RADAR_START_ADDR = 0x120 DELPHI_MRR_RADAR_MSG_COUNT = 64 -def _create_delphi_esr_radar_can_parser(): +def _create_delphi_esr_radar_can_parser(CP) -> CANParser: msg_n = len(DELPHI_ESR_RADAR_MSGS) signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, DELPHI_ESR_RADAR_MSGS * 3)) checks = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n)) - return CANParser(RADAR.DELPHI_ESR, signals, checks, CANBUS.radar) + return CANParser(RADAR.DELPHI_ESR, signals, checks, CanBus(CP).radar) -def _create_delphi_mrr_radar_can_parser(): +def _create_delphi_mrr_radar_can_parser(CP) -> CANParser: signals = [] checks = [] @@ -37,7 +38,7 @@ def _create_delphi_mrr_radar_can_parser(): ] checks += [(msg, 20)] - return CANParser(RADAR.DELPHI_MRR, signals, checks, CANBUS.radar) + return CANParser(RADAR.DELPHI_MRR, signals, checks, CanBus(CP).radar) class RadarInterface(RadarInterfaceBase): @@ -50,11 +51,11 @@ class RadarInterface(RadarInterfaceBase): if self.radar is None or CP.radarUnavailable: self.rcp = None elif self.radar == RADAR.DELPHI_ESR: - self.rcp = _create_delphi_esr_radar_can_parser() + self.rcp = _create_delphi_esr_radar_can_parser(CP) self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1] self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS} elif self.radar == RADAR.DELPHI_MRR: - self.rcp = _create_delphi_mrr_radar_can_parser() + self.rcp = _create_delphi_mrr_radar_can_parser(CP) self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1 else: raise ValueError(f"Unsupported radar: {self.radar}") diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 9b7b6ad83e..1807a26b17 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -38,12 +38,6 @@ class CarControllerParams: pass -class CANBUS: - main = 0 - radar = 1 - camera = 2 - - class CAR: BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN" ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"