📋📷 2019 Chrysler Pacifica and 2019 Jeep Grand Cherokee support (#590)

* 2019 Chrysler Pacfica and 2019 Jeep Grand Cherokee support, along with likely support for arbitrary models.
This is done by copying most values from the stock LKAS camera which is active with commaai/panda#177

* No longer send LKAS_HEARTBIT because Panda now forwards it for us.

* Pacifica Hybrid 2018 combine fingerprints, add 808: 8

* panda chrysler: forward bus 0 to bus 2
copy of Panda commit: commaai/panda@1049502

panda/board/safety/safety_chrysler.h

@@ -5,7 +5,7 @@ const int CHRYSLER_MAX_RATE_UP = 3;
 const int CHRYSLER_MAX_RATE_DOWN = 3;
 const int CHRYSLER_MAX_TORQUE_ERROR = 80;    // max torque cmd in excess of torque motor
 
-int chrysler_camera_detected = 0;
+int chrysler_camera_detected = 0;             // is giraffe switch 2 high?
 int chrysler_rt_torque_last = 0;
 int chrysler_desired_torque_last = 0;
 int chrysler_cruise_engaged_last = 0;
@@ -125,12 +125,29 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return true;
 }
 
+static void chrysler_init(int16_t param) {
+  chrysler_camera_detected = 0;
+}
+
+static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  int32_t addr = to_fwd->RIR >> 21;
+  // forward CAN 0 -> 2 so stock LKAS camera sees messages
+  if (bus_num == 0 && !chrysler_camera_detected) {
+    return 2;
+  }
+  // forward LKAS_HEARTBIT message from stock camera
+  if (bus_num == 2 && !chrysler_camera_detected && addr == 0x2d9) {
+    return 0;
+  }
+  return -1;  // do not forward
+}
+
 
 const safety_hooks chrysler_hooks = {
-  .init = nooutput_init,
+  .init = chrysler_init,
   .rx = chrysler_rx_hook,
   .tx = chrysler_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
   .ignition = default_ign_hook,
-  .fwd = nooutput_fwd_hook,
+  .fwd = chrysler_fwd_hook,
 };

selfdrive/car/chrysler/carcontroller.py

@@ -2,9 +2,9 @@
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
-                                               create_wheel_buttons, create_lkas_heartbit, \
+                                               create_wheel_buttons, \
                                                create_chimes
-from selfdrive.car.chrysler.values import ECU
+from selfdrive.car.chrysler.values import ECU, CAR
 from selfdrive.can.packer import CANPacker
 
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
@@ -29,6 +29,7 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera):
     self.car_fingerprint = car_fingerprint
     self.alert_active = False
     self.send_chime = False
+    self.gone_fast_yet = False
 
     self.fake_ecus = set()
     if enable_camera:
@@ -39,8 +40,8 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera):
 
   def update(self, sendcan, enabled, CS, frame, actuators,
              pcm_cancel_cmd, hud_alert, audible_alert):
-
     # this seems needed to avoid steering faults and to force the sync with the EPS counter
+    frame = CS.lkas_counter
     if self.prev_frame == frame:
       return
 
@@ -51,6 +52,11 @@ def update(self, sendcan, enabled, CS, frame, actuators,
                                                    CS.steer_torque_motor, SteerLimitParams)
 
     moving_fast = CS.v_ego > CS.CP.minSteerSpeed  # for status message
+    if CS.v_ego > (CS.CP.minSteerSpeed - 0.5):  # for command high bit
+      self.gone_fast_yet = True
+    elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
+      if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
+        self.gone_fast_yet = False  # < 14.5m/s stock turns off this bit, but fine down to 13.5
     lkas_active = moving_fast and enabled
 
     if not lkas_active:
@@ -76,18 +82,17 @@ def update(self, sendcan, enabled, CS, frame, actuators,
       new_msg = create_wheel_buttons(self.ccframe)
       can_sends.append(new_msg)
 
+    # LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
     # frame is 100Hz (0.01s period)
-    if (self.ccframe % 10 == 0):  # 0.1s period
-      new_msg = create_lkas_heartbit(self.car_fingerprint)
-      can_sends.append(new_msg)
-
     if (self.ccframe % 25 == 0):  # 0.25s period
-      new_msg = create_lkas_hud(self.packer, CS.gear_shifter, lkas_active, hud_alert, self.car_fingerprint,
-                                self.hud_count)
-      can_sends.append(new_msg)
-      self.hud_count += 1
-
-    new_msg = create_lkas_command(self.packer, int(apply_steer), frame)
+      if (CS.lkas_car_model != -1):
+        new_msg = create_lkas_hud(
+            self.packer, CS.gear_shifter, lkas_active, hud_alert,
+            self.hud_count, CS.lkas_car_model)
+        can_sends.append(new_msg)
+        self.hud_count += 1
+
+    new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
     can_sends.append(new_msg)
 
     self.ccframe += 1

selfdrive/car/chrysler/carstate.py

@@ -63,6 +63,18 @@ def get_can_parser(CP):
 
   return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
+def get_camera_parser(CP):
+  signals = [
+    # sig_name, sig_address, default
+    # TODO read in all the other values
+    ("COUNTER", "LKAS_COMMAND", -1),
+    ("CAR_MODEL", "LKAS_HUD", -1),
+    ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1)
+  ]
+  checks = []
+
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
+
 
 class CarState(object):
   def __init__(self, CP):
@@ -85,7 +97,7 @@ def __init__(self, CP):
     self.v_ego = 0.0
 
 
-  def update(self, cp):
+  def update(self, cp, cp_cam):
     # copy can_valid
     self.can_valid = cp.can_valid
 
@@ -142,3 +154,7 @@ def update(self, cp):
     self.pcm_acc_status = self.main_on
 
     self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
+
+    self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER']
+    self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL']
+    self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK']

selfdrive/car/chrysler/chryslercan.py

@@ -5,14 +5,6 @@
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
-MODEL_TO_CONSTANT = {
-  CAR.PACIFICA_2017_HYBRID: 0,
-  CAR.PACIFICA_2018: 0x64,
-  CAR.PACIFICA_2018_HYBRID: 0xa8,
-  CAR.PACIFICA_2019_HYBRID: 0x68,
-  CAR.JEEP_CHEROKEE: 0xa4,
-  }
-
 def calc_checksum(data):
   """This function does not want the checksum byte in the input data.
 
@@ -53,28 +45,26 @@ def calc_checksum(data):
 def make_can_msg(addr, dat):
   return [addr, 0, dat, 0]
 
-def create_lkas_heartbit(car_fingerprint):
-  # LKAS_HEARTBIT (729) Lane-keeping heartbeat.
-  msg = '0000000820'.decode('hex')  # 2017
-  return make_can_msg(0x2d9, msg)
+def create_lkas_heartbit(packer, lkas_status_ok):
+  # LKAS_HEARTBIT 0x2d9 (729) Lane-keeping heartbeat.
+  values = {
+    "LKAS_STATUS_OK": lkas_status_ok
+  }
+  return packer.make_can_msg("LKAS_HEARTBIT", 0, values)
 
-def create_lkas_hud(packer, gear, lkas_active, hud_alert, car_fingerprint, hud_count):
+def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model):
   # LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed.
 
   if hud_alert == VisualAlert.steerRequired:
     msg = '0000000300000000'.decode('hex')
     return make_can_msg(0x2a6, msg)
 
-  color = 0  # default values are for park or neutral
-  lines = 0
+  color = 1  # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019
+  lines = 1
   alerts = 0
 
-  if hud_count < (3 *4):  # first 3 seconds, 4Hz
-    lines = 1
+  if hud_count < (1 *4):  # first 3 seconds, 4Hz
     alerts = 1
-  elif hud_count < (6 * 4):  # next 3 seconds, 4Hz
-    lines = 1
-    alerts = 0
   # CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID
   # had color = 1 and lines = 1 but trying 2017 hybrid style for now.
   if gear in ('drive', 'reverse', 'low'):
@@ -87,19 +77,19 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, car_fingerprint, hud_c
 
   values = {
     "LKAS_ICON_COLOR": color,  # byte 0, last 2 bits
-    "CAR_MODEL": MODEL_TO_CONSTANT[car_fingerprint],  # byte 1
+    "CAR_MODEL": lkas_car_model,  # byte 1
     "LKAS_LANE_LINES": lines,  # byte 2, last 4 bits
     "LKAS_ALERTS": alerts,  # byte 3, last 4 bits
     }
 
   return packer.make_can_msg("LKAS_HUD", 0, values)  # 0x2a6
 
 
-def create_lkas_command(packer, apply_steer, frame):
-  # LKAS_COMMAND (658) Lane-keeping signal to turn the wheel.
+def create_lkas_command(packer, apply_steer, moving_fast, frame):
+  # LKAS_COMMAND 0x292 (658) Lane-keeping signal to turn the wheel.
   values = {
     "LKAS_STEERING_TORQUE": apply_steer,
-    "LKAS_HIGH_TORQUE": 1,
+    "LKAS_HIGH_TORQUE": int(moving_fast),
     "COUNTER": frame % 0x10,
   }
 

selfdrive/car/chrysler/chryslercan_test.py

@@ -1,5 +1,6 @@
 import chryslercan
 from values import CAR
+from carcontroller import CarController
 from selfdrive.can.packer import CANPacker
 
 from cereal import car
@@ -16,32 +17,52 @@ def test_checksum(self):
     self.assertEqual(0xcc, chryslercan.calc_checksum([0x14, 0, 0, 0, 0x20]))
 
   def test_heartbit(self):
+    packer = CANPacker('chrysler_pacifica_2017_hybrid')
     self.assertEqual(
         [0x2d9, 0, '0000000820'.decode('hex'), 0],
-        chryslercan.create_lkas_heartbit(CAR.PACIFICA_2017_HYBRID))
+        chryslercan.create_lkas_heartbit(packer, 0x820))
 
   def test_hud(self):
     packer = CANPacker('chrysler_pacifica_2017_hybrid')
     self.assertEqual(
-        [0x2a6, 0, '0000010100000000'.decode('hex'), 0],
-        chryslercan.create_lkas_hud(packer,
-            'park', False, False, CAR.PACIFICA_2017_HYBRID, 1))
+        [0x2a6, 0, '0100010100000000'.decode('hex'), 0],
+        chryslercan.create_lkas_hud(
+            packer,
+            'park', False, False, 1, 0))
     self.assertEqual(
-        [0x2a6, 0, '0000010000000000'.decode('hex'), 0],
-        chryslercan.create_lkas_hud(packer,
-            'park', False, False, CAR.PACIFICA_2017_HYBRID, 5*4))
+        [0x2a6, 0, '0100010000000000'.decode('hex'), 0],
+        chryslercan.create_lkas_hud(
+            packer,
+            'park', False, False, 5*4, 0))
     self.assertEqual(
-        [0x2a6, 0, '0000000000000000'.decode('hex'), 0],
-        chryslercan.create_lkas_hud(packer,
-            'park', False, False, CAR.PACIFICA_2017_HYBRID, 99999))
+        [0x2a6, 0, '0100010000000000'.decode('hex'), 0],
+        chryslercan.create_lkas_hud(
+            packer,
+            'park', False, False, 99999, 0))
     self.assertEqual(
         [0x2a6, 0, '0200060000000000'.decode('hex'), 0],
-        chryslercan.create_lkas_hud(packer,
-            'drive', True, False, CAR.PACIFICA_2017_HYBRID, 99999))
+        chryslercan.create_lkas_hud(
+            packer,
+            'drive', True, False, 99999, 0))
     self.assertEqual(
         [0x2a6, 0, '0264060000000000'.decode('hex'), 0],
-        chryslercan.create_lkas_hud(packer,
-            'drive', True, False, CAR.PACIFICA_2018, 99999))
+        chryslercan.create_lkas_hud(
+            packer,
+            'drive', True, False, 99999, 0x64))
+
+  def test_command(self):
+    packer = CANPacker('chrysler_pacifica_2017_hybrid')
+    self.assertEqual(
+        [0x292, 0, '140000001086'.decode('hex'), 0],
+        chryslercan.create_lkas_command(
+            packer,
+            0, True, 1))
+    self.assertEqual(
+        [0x292, 0, '040000008083'.decode('hex'), 0],
+        chryslercan.create_lkas_command(
+            packer,
+            0, False, 8))
+
 
 if __name__ == '__main__':
   unittest.main()

selfdrive/car/chrysler/interface.py

@@ -4,7 +4,7 @@
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
 from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.car.chrysler.carstate import CarState, get_can_parser
+from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser
 from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
 
 try:
@@ -27,8 +27,8 @@ def __init__(self, CP, sendcan=None):
 
     # *** init the major players ***
     self.CS = CarState(CP)
-
     self.cp = get_can_parser(CP)
+    self.cp_cam = get_camera_parser(CP)
 
     # sending if read only is False
     if sendcan is not None:
@@ -79,7 +79,7 @@ def get_params(candidate, fingerprint):
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.7
 
-    if candidate == CAR.JEEP_CHEROKEE:
+    if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
       ret.wheelbase = 2.91  # in meters
       ret.steerRatio = 12.7
       ret.steerActuatorDelay = 0.2  # in seconds
@@ -91,6 +91,9 @@ def get_params(candidate, fingerprint):
 
     ret.minSteerSpeed = 3.8  # m/s
     ret.minEnableSpeed = -1.   # enable is done by stock ACC, so ignore this
+    if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
+      ret.minSteerSpeed = 17.5  # m/s 17 on the way up, 13 on the way down once engaged.
+      # TODO allow 2019 cars to steer down to 13 m/s if already engaged.
 
     centerToRear = ret.wheelbase - ret.centerToFront
     # TODO: get actual value, for now starting with reasonable value for
@@ -137,10 +140,9 @@ def get_params(candidate, fingerprint):
   def update(self, c):
     # ******************* do can recv *******************
     canMonoTimes = []
-
     self.cp.update(int(sec_since_boot() * 1e9), False)
-
-    self.CS.update(self.cp)
+    self.cp_cam.update(int(sec_since_boot() * 1e9), False)
+    self.CS.update(self.cp, self.cp_cam)
 
     # create message
     ret = car.CarState.new_message()
@@ -208,6 +210,8 @@ def update(self, c):
     self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
 
     ret.genericToggle = self.CS.generic_toggle
+    #ret.lkasCounter = self.CS.lkas_counter
+    #ret.lkasCarModel = self.CS.lkas_car_model
 
     # events
     events = []