From ef9deeb6eba3823982dadd485a3676441320226b Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Mon, 30 Jan 2023 22:37:54 -0800 Subject: [PATCH 1/3] Ford longitudinal control (#27161) ford long --- selfdrive/car/ford/carcontroller.py | 19 ++++++++++++++++++- selfdrive/car/ford/fordcan.py | 20 ++++++++++++++++++++ selfdrive/car/ford/values.py | 5 +++++ 3 files changed, 43 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 06da87bf34..cfff37f6d5 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -2,7 +2,8 @@ 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_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg +from selfdrive.car.ford.fordcan import create_acc_command, 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 VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -55,6 +56,22 @@ class CarController: can_sends.append(create_lka_msg(self.packer)) can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + ### longitudinal control ### + # send acc command at 50Hz + if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + precharge_brake = accel < -0.1 + if accel > -0.5: + gas = accel + decel = False + else: + gas = -5.0 + decel = True + + can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) + + ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 9ddde80f87..d9c37426f8 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -55,6 +55,26 @@ def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) +def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): + """ + Creates a CAN message for the Ford ACC Command. + + This command can be used to enable ACC, to set the ACC gas/brake/decel values + and to disable ACC. + + Frequency is 50Hz. + """ + + values = { + "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 + "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes + "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 + "AccBrkPrchg_B_Rq": 1 if precharge_brake else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + } + return packer.make_can_msg("ACCDATA", CANBUS.main, values) + + 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. diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index b93be61448..7b075f1547 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -14,6 +14,8 @@ Ecu = car.CarParams.Ecu class CarControllerParams: # Messages: Lane_Assist_Data1, LateralMotionControl STEER_STEP = 5 + # Message: ACCDATA + ACC_CONTROL_STEP = 2 # Message: IPMA_Data LKAS_UI_STEP = 100 # Message: ACCDATA_3 @@ -29,6 +31,9 @@ class CarControllerParams: ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.005, 0.00056, 0.0002]) ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.008, 0.00089, 0.00032]) + ACCEL_MAX = 2.0 # m/s^s max acceleration + ACCEL_MIN = -3.5 # m/s^s max deceleration + def __init__(self, CP): pass From 4c2f8edb7783f4b5797c02f1bdc2008fbaf4e0c7 Mon Sep 17 00:00:00 2001 From: Anthony Rose <20302208+Cx01N@users.noreply.github.com> Date: Tue, 31 Jan 2023 02:23:45 -0500 Subject: [PATCH 2/3] Add Hyundai Ioniq 5 Fingerprint (#27137) * added 2022 ioniq 5 fingerprint * removed extra eps field * fixed eps value * removed cornerradar * Apply suggestions from code review --------- Co-authored-by: Shane Smiskol --- selfdrive/car/hyundai/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index c0cf23b50d..4e3dd484c8 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1493,6 +1493,7 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', + b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007', ], }, CAR.TUCSON_4TH_GEN: { From 09cd0b4900d8aff1c9484c8848c9b5f7ff269594 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 31 Jan 2023 01:09:36 -0800 Subject: [PATCH 3/3] GM camera ACC: reliable relay open init (#27163) Reliable relay open --- selfdrive/car/gm/carcontroller.py | 2 +- selfdrive/car/gm/carstate.py | 15 +++++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b4a79d10a6..c5aae34249 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -63,7 +63,7 @@ class CarController: elif (self.frame - self.last_steer_frame) >= steer_step: # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus if init_lka_counter: - self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1 + self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index de0fd2eed6..cf6d2817ec 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -18,7 +18,7 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] self.loopback_lka_steering_cmd_updated = False - self.camera_lka_steering_cmd_counter = 0 + self.pt_lka_steering_cmd_counter = 0 self.buttons_counter = 0 def update(self, pt_cp, cam_cp, loopback_cp): @@ -33,7 +33,7 @@ class CarState(CarStateBase): # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 if self.CP.networkLocation == NetworkLocation.fwdCamera: - self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] + self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], @@ -113,13 +113,11 @@ class CarState(CarStateBase): if CP.networkLocation == NetworkLocation.fwdCamera: signals += [ ("AEBCmdActive", "AEBCmd"), - ("RollingCounter", "ASCMLKASteeringCmd"), ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"), ("ACCCruiseState", "ASCMActiveCruiseControlStatus"), ] checks += [ ("AEBCmd", 10), - ("ASCMLKASteeringCmd", 10), ("ASCMActiveCruiseControlStatus", 25), ] @@ -180,6 +178,15 @@ class CarState(CarStateBase): ("ECMAcceleratorPos", 80), ] + # Used to read back last counter sent to PT by camera + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ] + checks += [ + ("ASCMLKASteeringCmd", 0), + ] + if CP.transmissionType == TransmissionType.direct: signals.append(("RegenPaddle", "EBCMRegenPaddle")) checks.append(("EBCMRegenPaddle", 50))