mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ford: radar + long control fixups (#28948)

Browse Source 
* 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 <adeebshihadeh@gmail.com>
old-commit-hash: 8c25741e468b7cb11a59c8905c9d00d5612b9143
This commit is contained in:
Harald Schäfer
2023-12-14 03:54:06 +00:00
committed by GitHub
parent b48abb9511
commit 2d7a222239
3 changed files with 17 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/car/ford/carcontroller.py
View File

@@ -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)

4
selfdrive/car/ford/fordcan.py
View File

@@ -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

18
selfdrive/car/ford/radar_interface.py
View File

@@ -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