From 4ee6c68ba5a61362481c8189f8a89e31fd09d390 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Thu, 4 May 2023 21:04:54 +0000 Subject: [PATCH] Ford: cleanup long control and send stopping signal (#28110) Co-authored-by: Shane Smiskol --- selfdrive/car/ford/carcontroller.py | 13 ++++++------- selfdrive/car/ford/fordcan.py | 6 ++++-- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index d205960605..edce3418be 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -6,6 +6,7 @@ from selfdrive.car.ford.fordcan import create_acc_msg, create_acc_ui_msg, create create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams +LongCtrlState = car.CarControl.Actuators.LongControlState VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -83,15 +84,13 @@ class CarController: 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 = accel + decel = accel < 0.0 + if accel < -0.5: gas = -5.0 - decel = True - can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) + stopping = CC.actuators.longControlState == LongCtrlState.stopping + can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, decel, stopping)) ### 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 b56a3185c2..81e08c3671 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -97,7 +97,7 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) -def create_acc_msg(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): +def create_acc_msg(packer, long_active: bool, gas: float, accel: float, decel: bool, stopping: bool): """ Creates a CAN message for the Ford ACC Command. @@ -111,8 +111,10 @@ def create_acc_msg(packer, long_active: bool, gas: float, accel: float, precharg "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 + "AccResumEnbl_B_Rq": 1 if long_active else 0, + "AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes "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)