From 8c25741e468b7cb11a59c8905c9d00d5612b9143 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Thu, 14 Dec 2023 03:54:06 +0000 Subject: [PATCH] Ford: radar + long control fixups (#28948) * enable radar * manual new track * Don't filter points * cleanup * Update radar_interface.py * This is non-zero in stock * add speed * more extreme * A few more things * fixes * cleanup * revert that --------- Co-authored-by: Adeeb Shihadeh --- selfdrive/car/ford/carcontroller.py | 4 ++-- selfdrive/car/ford/fordcan.py | 4 +++- selfdrive/car/ford/radar_interface.py | 18 ++++++++++++------ 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index fe9ee404a4..88ec6a5072 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,5 +1,6 @@ from cereal import car from openpilot.common.numpy_fast import clip +from openpilot.common.conversions import Conversions as CV from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan @@ -88,9 +89,8 @@ class CarController: gas = accel if not CC.longActive or gas < CarControllerParams.MIN_GAS: gas = CarControllerParams.INACTIVE_GAS - stopping = CC.actuators.longControlState == LongCtrlState.stopping - can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping)) + can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=40 * CV.MPH_TO_KPH)) ### 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 e0086ecbb5..78a249e387 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -118,7 +118,7 @@ def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path return packer.make_can_msg("LateralMotionControl2", CAN.main, values) -def create_acc_msg(packer, CAN: CanBus, 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, v_ego_kph: float): """ Creates a CAN message for the Ford ACC Command. @@ -133,7 +133,9 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl "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 + "AccPrpl_A_Pred": gas, # Acceleration request: [-5|5.23] m/s^2 "AccResumEnbl_B_Rq": 1 if long_active else 0, + "AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h # TODO: we may be able to improve braking response by utilizing pre-charging better "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 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 716c9b6e58..209bbebae3 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -119,17 +119,23 @@ class RadarInterface(RadarInterfaceBase): self.track_id += 1 valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"]) - amplitude = msg[f"CAN_DET_AMPLITUDE_{ii:02d}"] # dBsm [-64|63] - if valid and 0 < amplitude <= 15: + if valid: azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964] dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984] distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984] + dRel = cos(azimuth) * dist # m from front of car + yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive - # *** openpilot radar point *** - self.pts[i].dRel = cos(azimuth) * dist # m from front of car - self.pts[i].yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive - self.pts[i].vRel = distRate # m/s + # delphi doesn't notify of track switches, so do it manually + # TODO: refactor this to radard if more radars behave this way + if abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5: + self.track_id += 1 + self.pts[i].trackId = self.track_id + + self.pts[i].dRel = dRel + self.pts[i].yRel = yRel + self.pts[i].vRel = distRate self.pts[i].measured = True