Merge remote-tracking branch 'upstream/master' into ford-lsl

opendbc/car/__init__.py

@@ -21,6 +21,8 @@
 # kg of standard extra cargo to count for drive, gas, etc...
 STD_CARGO_KG = 136.
 
+ACCELERATION_DUE_TO_GRAVITY = 9.81  # m/s^2
+
 ButtonType = structs.CarState.ButtonEvent.Type
 AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])
 

opendbc/car/car.capnp

@@ -508,7 +508,6 @@ struct CarParams {
   transmissionType @43 :TransmissionType;
   carFw @44 :List(CarFw);
 
-  radarTimeStep @45: Float32 = 0.05;  # time delta between radar updates, 20Hz is very standard
   radarDelay @74 :Float32;
   fingerprintSource @49: FingerprintSource;
   networkLocation @50 :NetworkLocation;  # Where Panda/C2 is integrated into the car's CAN network
@@ -719,4 +718,5 @@ struct CarParams {
   maxSteeringAngleDegDEPRECATED @54 :Float32;
   longitudinalActuatorDelayLowerBoundDEPRECATED @61 :Float32;
   stoppingControlDEPRECATED @31 :Bool; # Does the car allow full control even at lows speeds when stopping
+  radarTimeStepDEPRECATED @45: Float32 = 0.05;  # time delta between radar updates, 20Hz is very standard
 }

opendbc/car/chrysler/fingerprints.py

@@ -325,6 +325,7 @@
       b'68402703AB',
       b'68402704AB',
       b'68402708AB',
+      b'68402714AB',
       b'68402971AD',
       b'68454144AD',
       b'68454145AB',
@@ -362,6 +363,7 @@
     ],
     (Ecu.engine, 0x7e0, None): [
       b'05035674AB ',
+      b'68412635AE ',
       b'68412635AG ',
       b'68412660AD ',
       b'68422860AB',
@@ -561,6 +563,7 @@
       b'68455145AE ',
       b'68455146AC ',
       b'68460927AA ',
+      b'68467909AB ',
       b'68467915AC ',
       b'68467916AC ',
       b'68467936AC ',
@@ -611,6 +614,7 @@
       b'68445536AB',
       b'68445537AB',
       b'68466081AB',
+      b'68466086AB',
       b'68466087AB',
       b'68484466AC',
       b'68484467AC',

opendbc/car/ford/carcontroller.py

@@ -1,8 +1,9 @@
+import math
 from opendbc.can.packer import CANPacker
-from opendbc.car import apply_std_steer_angle_limits, structs
+from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, apply_std_steer_angle_limits, structs
 from opendbc.car.ford import fordcan
 from opendbc.car.ford.values import CarControllerParams, FordFlags
-from opendbc.car.common.numpy_fast import clip
+from opendbc.car.common.numpy_fast import clip, interp
 from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
 
 LongCtrlState = structs.CarControl.Actuators.LongControlState
@@ -21,17 +22,28 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
   return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
 
 
+def apply_creep_compensation(accel: float, v_ego: float) -> float:
+  creep_accel = interp(v_ego, [1., 3.], [0.6, 0.])
+  if accel < 0.:
+    accel -= creep_accel
+  return accel
+
+
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP):
     super().__init__(dbc_name, CP)
     self.packer = CANPacker(dbc_name)
     self.CAN = fordcan.CanBus(CP)
 
     self.apply_curvature_last = 0
+    self.accel = 0.0
+    self.gas = 0.0
+    self.brake_request = False
     self.main_on_last = False
     self.lkas_enabled_last = False
     self.steer_alert_last = False
     self.lead_distance_bars_last = None
+    self.distance_bar_frame = 0
 
   def update(self, CC, CS, now_nanos):
     can_sends = []
@@ -87,14 +99,34 @@ def update(self, CC, CS, now_nanos):
     ### longitudinal control ###
     # send acc msg at 50Hz
     if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
+      # Compensate for engine creep at low speed.
+      # Either the ABS does not account for engine creep, or the correction is very slow
+      # TODO: verify this applies to EV/hybrid
+      self.accel = actuators.accel
+      if CC.longActive:
+        self.accel = apply_creep_compensation(self.accel, CS.out.vEgo)
+      self.accel = clip(self.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
       # Both gas and accel are in m/s^2, accel is used solely for braking
-      accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-      gas = accel
-      if not CC.longActive or gas < CarControllerParams.MIN_GAS:
-        gas = CarControllerParams.INACTIVE_GAS
+      self.gas = self.accel
+      if not CC.longActive or self.gas < CarControllerParams.MIN_GAS:
+        self.gas = CarControllerParams.INACTIVE_GAS
+
+      # PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
+      if len(CC.orientationNED) == 3:
+        accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
+      else:
+        accel_due_to_pitch = 0.0
+
+      accel_pitch_compensated = self.accel + accel_due_to_pitch
+      if accel_pitch_compensated > 0.3 or not CC.longActive:
+        self.brake_request = False
+      elif accel_pitch_compensated < 0.0:
+        self.brake_request = True
+
       stopping = CC.actuators.longControlState == LongCtrlState.stopping
       # TODO: look into using the actuators packet to send the desired speed
-      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=V_CRUISE_MAX))
+      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, self.gas, self.accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
@@ -105,10 +137,13 @@ def update(self, CC, CS, now_nanos):
     # send acc ui msg at 5Hz or if ui state changes
     if hud_control.leadDistanceBars != self.lead_distance_bars_last:
       send_ui = True
+      self.distance_bar_frame = self.frame
+
     if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
+      show_distance_bars = self.frame - self.distance_bar_frame < 400
       can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
-                                                 fcw_alert, CS.out.cruiseState.standstill, hud_control,
-                                                 CS.acc_tja_status_stock_values))
+                                                 fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
+                                                 hud_control, CS.acc_tja_status_stock_values))
 
     self.main_on_last = main_on
     self.lkas_enabled_last = CC.latActive
@@ -117,6 +152,8 @@ def update(self, CC, CS, now_nanos):
 
     new_actuators = actuators.as_builder()
     new_actuators.curvature = self.apply_curvature_last
+    new_actuators.accel = self.accel
+    new_actuators.gas = self.gas
 
     self.frame += 1
     return new_actuators, can_sends

opendbc/car/ford/carstate.py

@@ -76,6 +76,8 @@ def update(self, cp, cp_cam, *_) -> structs.CarState:
       else:
         ret.gearShifter = GearShifter.drive
 
+    ret.engineRpm = cp.vl["EngVehicleSpThrottle"]["EngAout_N_Actl"]
+
     # safety
     ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
     ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])

opendbc/car/ford/fordcan.py

@@ -117,7 +117,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, v_ego_kph: float):
+def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float):
   """
   Creates a CAN message for the Ford ACC Command.
 
@@ -126,24 +126,27 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl
 
   Frequency is 50Hz.
   """
-  decel = accel < 0 and long_active
   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
+    # No observed acceleration seen from this signal alone. During stock system operation, it appears to
+    # be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative)
     "AccPrpl_A_Pred": -5.0,                           # Acceleration request: [-5|5.23] m/s^2
     "AccResumEnbl_B_Rq": 1 if long_active else 0,
+    # No observed acceleration seen from this signal alone
     "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
+    # When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq
+    "AccBrkPrchg_B_Rq": 1 if brake_request else 0,            # Pre-charge brake request: 0=No, 1=Yes
+    "AccBrkDecel_B_Rq": 1 if brake_request else 0,            # Deceleration request: 0=Inactive, 1=Active
     "AccStopStat_B_Rq": 1 if stopping else 0,
   }
   return packer.make_can_msg("ACCDATA", CAN.main, values)
 
 
 def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
-                      hud_control, stock_values: dict):
+                      show_distance_bars: bool, hud_control, stock_values: dict):
   """
   Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
   assist status.
@@ -207,7 +210,7 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw
     values.update({
       "AccStopStat_D_Dsply": 2 if standstill else 0,              # Stopping status text
       "AccMsgTxt_D2_Rq": 0,                                       # ACC text
-      "AccTGap_B_Dsply": 0,                                       # Show time gap control UI
+      "AccTGap_B_Dsply": 1 if show_distance_bars else 0,          # Show time gap control UI
       "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0,  # Lead indicator
       "AccStopMde_B_Dsply": 1 if standstill else 0,
       "AccWarn_D_Dsply": 0,                                       # ACC warning

opendbc/car/ford/interface.py

@@ -2,7 +2,7 @@
 from opendbc.car import get_safety_config, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.ford.fordcan import CanBus
-from opendbc.car.ford.values import Ecu, FordFlags
+from opendbc.car.ford.values import DBC, Ecu, FordFlags, RADAR
 from opendbc.car.interfaces import CarInterfaceBase
 
 TransmissionType = structs.CarParams.TransmissionType
@@ -19,6 +19,11 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
     ret.steerActuatorDelay = 0.2
     ret.steerLimitTimer = 1.0
 
+    if DBC[candidate]['radar'] == RADAR.DELPHI_MRR:
+      # average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
+      # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms
+      ret.radarDelay = 0.06
+
     CAN = CanBus(fingerprint=fingerprint)
     cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)]
     if CAN.main >= 4:

opendbc/car/ford/radar_interface.py

@@ -9,8 +9,12 @@
 DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
 
 DELPHI_MRR_RADAR_START_ADDR = 0x120
+DELPHI_MRR_RADAR_HEADER_ADDR = 0x174  # MRR_Header_SensorCoverage
 DELPHI_MRR_RADAR_MSG_COUNT = 64
 
+DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175}  # scan index to detection range (m)
+MIN_LONG_RANGE_DIST = 30  # meters
+
 
 def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
   msg_n = len(DELPHI_ESR_RADAR_MSGS)
@@ -20,11 +24,14 @@ def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
 
 
 def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
-  messages = []
+  messages = [
+    ("MRR_Header_InformationDetections", 33),
+    ("MRR_Header_SensorCoverage", 33),
+  ]
 
   for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
     msg = f"MRR_Detection_{i:03d}"
-    messages += [(msg, 20)]
+    messages += [(msg, 33)]
 
   return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
 
@@ -36,15 +43,15 @@ def __init__(self, CP):
     self.updated_messages = set()
     self.track_id = 0
     self.radar = DBC[CP.carFingerprint]['radar']
-    if self.radar is None or CP.radarUnavailable:
+    if CP.radarUnavailable:
       self.rcp = None
     elif self.radar == RADAR.DELPHI_ESR:
       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(CP)
-      self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1
+      self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR
     else:
       raise ValueError(f"Unsupported radar: {self.radar}")
 
@@ -62,14 +69,14 @@ def update(self, can_strings):
     errors = []
     if not self.rcp.can_valid:
       errors.append("canError")
-    ret.errors = errors
 
     if self.radar == RADAR.DELPHI_ESR:
       self._update_delphi_esr()
     elif self.radar == RADAR.DELPHI_MRR:
-      self._update_delphi_mrr()
+      errors.extend(self._update_delphi_mrr())
 
     ret.points = list(self.pts.values())
+    ret.errors = errors
     self.updated_messages.clear()
     return ret
 
@@ -103,14 +110,26 @@ def _update_delphi_esr(self):
           del self.pts[ii]
 
   def _update_delphi_mrr(self):
+    headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11
+
+    errors = []
+    if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
+      errors.append("wrongConfig")
+
     for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
       msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
 
-      # SCAN_INDEX rotates through 0..3 on each message
-      # treat these as separate points
+      # SCAN_INDEX rotates through 0..3 on each message for different measurement modes
+      # Indexes 0 and 2 have a max range of ~40m, 1 and 3 are ~170m (MRR_Header_SensorCoverage->CAN_RANGE_COVERAGE)
+      # Indexes 0 and 1 have a Doppler coverage of +-71 m/s, 2 and 3 have +-60 m/s
+      # TODO: can we group into 2 groups?
       scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
       i = (ii - 1) * 4 + scanIndex
 
+      # Throw out old measurements. Very unlikely to happen, but is proper behavior
+      if scanIndex != headerScanIndex:
+        continue
+
       if i not in self.pts:
         self.pts[i] = structs.RadarData.RadarPoint()
         self.pts[i].trackId = self.track_id
@@ -120,9 +139,13 @@ def _update_delphi_mrr(self):
 
       valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
 
+      # Long range measurement mode is more sensitive and can detect the road surface
+      dist = msg[f"CAN_DET_RANGE_{ii:02d}"]  # m [0|255.984]
+      if scanIndex in (1, 3) and dist < MIN_LONG_RANGE_DIST:
+        valid = False
+
       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
@@ -141,3 +164,5 @@ def _update_delphi_mrr(self):
 
       else:
         del self.pts[i]
+
+    return errors

opendbc/car/gm/interface.py

@@ -138,7 +138,6 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
 
     ret.steerLimitTimer = 0.4
-    ret.radarTimeStep = 0.0667  # GM radar runs at 15Hz instead of standard 20Hz
     ret.longitudinalActuatorDelay = 0.5  # large delay to initially start braking
 
     if candidate == CAR.CHEVROLET_VOLT:

opendbc/car/gm/radar_interface.py

@@ -40,7 +40,6 @@ def __init__(self, CP):
 
     self.trigger_msg = LAST_RADAR_MSG
     self.updated_messages = set()
-    self.radar_ts = CP.radarTimeStep
 
   def update(self, can_strings):
     if self.rcp is None:

opendbc/car/honda/fingerprints.py

@@ -439,6 +439,7 @@
       b'36161-TPA-E050\x00\x00',
       b'36161-TPG-A030\x00\x00',
       b'36161-TPG-A040\x00\x00',
+      b'36161-TPG-A050\x00\x00',
     ],
     (Ecu.fwdRadar, 0x18dab0f1, None): [
       b'36802-TMB-H040\x00\x00',

opendbc/car/honda/radar_interface.py

@@ -18,7 +18,6 @@ def __init__(self, CP):
     self.radar_fault = False
     self.radar_wrong_config = False
     self.radar_off_can = CP.radarUnavailable
-    self.radar_ts = CP.radarTimeStep
 
     # Nidec
     if self.radar_off_can:

opendbc/car/honda/values.py

@@ -159,7 +159,7 @@ class CAR(Platforms):
     flags=HondaFlags.BOSCH_ALT_BRAKE,
   )
   HONDA_CRV_HYBRID = HondaBoschPlatformConfig(
-    [HondaCarDocs("Honda CR-V Hybrid 2017-21", min_steer_speed=12. * CV.MPH_TO_MS)],
+    [HondaCarDocs("Honda CR-V Hybrid 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)],
     # mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end
     CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677),
     dbc_dict('honda_accord_2018_can_generated', None),