Merge branch 'commaai:master' into master

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

@@ -131,6 +131,7 @@
       b'68496652AH ',
       b'68526752AD ',
       b'68526752AE ',
+      b'68526754AD ',
       b'68526754AE ',
       b'68536264AE ',
       b'68700304AB ',
@@ -301,17 +302,20 @@
       b'68252272AG ',
       b'68284455AI ',
       b'68284456AI ',
+      b'68284456AJ ',
       b'68284477AF ',
       b'68325564AH ',
       b'68325564AI ',
       b'68325565AH ',
       b'68325565AI ',
+      b'68325565AJ ',
       b'68325618AD ',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'05035517AH',
       b'68253222AF',
       b'68311218AC',
+      b'68311218AD',
       b'68311223AF',
       b'68311223AG',
       b'68361911AE',
@@ -325,6 +329,7 @@
       b'68402703AB',
       b'68402704AB',
       b'68402708AB',
+      b'68402714AB',
       b'68402971AD',
       b'68454144AD',
       b'68454145AB',
@@ -359,9 +364,11 @@
       b'68499171AA',
       b'68499171AB',
       b'68501183AA',
+      b'68501186AA',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'05035674AB ',
+      b'68412635AE ',
       b'68412635AG ',
       b'68412660AD ',
       b'68422860AB',
@@ -381,6 +388,7 @@
       b'68495807AA',
       b'68495807AB',
       b'68503641AC',
+      b'68503644AC',
       b'68503664AC',
     ],
   },
@@ -435,6 +443,7 @@
       b'68527397AD',
       b'68527403AC',
       b'68527403AD',
+      b'68527404AD',
       b'68546047AF',
       b'68631938AA',
       b'68631939AA',
@@ -488,6 +497,7 @@
     (Ecu.eps, 0x75a, None): [
       b'21590101AA',
       b'21590101AB',
+      b'22490101AB',
       b'68273275AF',
       b'68273275AG',
       b'68273275AH',
@@ -521,6 +531,7 @@
       b'05035841AC ',
       b'05035841AD ',
       b'05036026AB ',
+      b'05036030AC ',
       b'05036065AE ',
       b'05036066AE ',
       b'05036067AE ',
@@ -561,6 +572,7 @@
       b'68455145AE ',
       b'68455146AC ',
       b'68460927AA ',
+      b'68467909AB ',
       b'68467915AC ',
       b'68467916AC ',
       b'68467936AC ',
@@ -611,6 +623,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, DT_CTRL, 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,42 @@ 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:
-      # 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)
+      accel = actuators.accel
       gas = accel
+
+      if CC.longActive:
+        # 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
+        accel = apply_creep_compensation(accel, CS.out.vEgo)
+
+        # The stock system has been seen rate limiting the brake accel to 5 m/s^3,
+        # however even 3.5 m/s^3 causes some overshoot with a step response.
+        accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL))
+
+      accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
+      # Both gas and accel are in m/s^2, accel is used solely for braking
       if not CC.longActive or gas < CarControllerParams.MIN_GAS:
         gas = CarControllerParams.INACTIVE_GAS
+
+      # PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
+      accel_due_to_pitch = 0.0
+      if len(CC.orientationNED) == 3:
+        accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
+
+      accel_pitch_compensated = 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, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
+
+      self.accel = accel
+      self.gas = gas
 
     ### 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 +145,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 +160,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
@@ -14,11 +14,19 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
     ret.carName = "ford"
     ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD)
 
-    ret.radarUnavailable = True
+    ret.radarUnavailable = DBC[candidate]['radar'] is None
     ret.steerControlType = structs.CarParams.SteerControlType.angle
     ret.steerActuatorDelay = 0.2
     ret.steerLimitTimer = 1.0
 
+    ret.longitudinalTuning.kiBP = [0.]
+    ret.longitudinalTuning.kiV = [0.5]
+
+    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: