Chrysler

opendbc/chrysler_pacifica_2017_hybrid.dbc

@@ -206,6 +206,7 @@ BO_ 571 WHEEL_BUTTONS: 3 XXX
  SG_ ACC_SPEED_INC : 2|1@0+ (1,0) [0|255] "" XXX
  SG_ ACC_SPEED_DEC : 3|1@1+ (1,0) [0|3] "" XXX
  SG_ ACC_FOLLOW_INC : 8|1@1+ (1,0) [0|15] "" XXX
+ SG_ ACC_CANCEL : 0|1@0+ (1,0) [0|15] "" XXX
  SG_ COUNTER : 15|4@0+ (1,0) [0|15] "" XXX
 
 BO_ 669 NEW_MSG_29d: 3 XXX
@@ -402,6 +403,7 @@ CM_ SG_ 729 LKAS_STATUS_OK "Set to 0x0820 when LKAS system is plugged in.";
 CM_ SG_ 288 UNKNOWN_CHECKSUM_120 "not the LKAS checksum";
 CM_ SG_ 288 GAS_ENGINE_RPM_MAYBE "lags acceleration, perhaps gas engine";
 CM_ SG_ 257 ENERGY_OR_RPM "perhaps energy consumption or RPMs";
+CM_ SG_ 571 CHECKSUM_23B "standard checksum";
 CM_ SG_ 270 ELECTRIC_MOTOR "0x7fff indicates electric motor not in use";
 CM_ SG_ 291 ENERGY_GAIN_LOSS "unsure what this actually is";
 CM_ SG_ 291 ENERGY_SMOOTHER_CURVE "unusre what it is, but smoother";

selfdrive/boardd/boardd.cc

@@ -41,6 +41,7 @@
 #define SAFETY_CADILLAC 6
 #define SAFETY_HYUNDAI 7
 #define SAFETY_TESLA 8
+#define SAFETY_CHRYSLER 9
 #define SAFETY_TOYOTA_IPAS 0x1335
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
@@ -120,6 +121,9 @@ void *safety_setter_thread(void *s) {
   case (int)cereal::CarParams::SafetyModels::HYUNDAI:
     safety_setting = SAFETY_HYUNDAI;
     break;
+  case (int)cereal::CarParams::SafetyModels::CHRYSLER:
+    safety_setting = SAFETY_CHRYSLER;
+    break;
   default:
     LOGE("unknown safety model: %d", safety_model);
   }
@@ -420,6 +424,14 @@ void *can_send_thread(void *crap) {
   zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
   zmq_connect(subscriber, "tcp://127.0.0.1:8017");
 
+  // drain sendcan to delete any stale messages from previous runs
+  zmq_msg_t msg;
+  zmq_msg_init(&msg);
+  int err = 0;
+  while(err >= 0) {
+    err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
+  }
+
   // run as fast as messages come in
   while (!do_exit) {
     can_send(subscriber);

selfdrive/boardd/boardd.py

@@ -1,5 +1,8 @@
 #!/usr/bin/env python
 
+# This file is not used by OpenPilot. Only boardd.cc is used.
+# The python version is slower, but has more options for development.
+
 # TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and
 # delete this python version of boardd
 
@@ -23,6 +26,7 @@
 SAFETY_NOOUTPUT = 0
 SAFETY_HONDA = 1
 SAFETY_TOYOTA = 2
+SAFETY_CHRYSLER = 8
 SAFETY_TOYOTA_NOLIMITS = 0x1336
 SAFETY_ALLOUTPUT = 0x1337
 
@@ -163,6 +167,9 @@ def boardd_loop(rate=200):
   # *** subscribes to can send
   sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
 
+  # drain sendcan to delete any stale messages from previous runs
+  messaging.drain_sock(sendcan)
+
   while 1:
     # health packet @ 1hz
     if (rk.frame%rate) == 0:
@@ -205,6 +212,9 @@ def boardd_proxy_loop(rate=200, address="192.168.2.251"):
   # *** publishes to can send
   sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
 
+  # drain sendcan to delete any stale messages from previous runs
+  messaging.drain_sock(sendcan)
+
   while 1:
     # recv @ 100hz
     can_msgs = can_recv()

selfdrive/can/parser.cc

@@ -177,6 +177,14 @@ class CANParser {
 
     zmq_connect(subscriber, tcp_addr_char);
 
+    // drain sendcan to delete any stale messages from previous runs
+    zmq_msg_t msgDrain;
+    zmq_msg_init(&msgDrain);
+    int err = 0;
+    while(err >= 0) {
+      err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT);
+    }
+
     dbc = dbc_lookup(dbc_name);
     assert(dbc);
 

selfdrive/car/chrysler/carcontroller.py

@@ -0,0 +1,79 @@
+import logging
+from common.numpy_fast import clip, interp
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.chrysler.chryslercan import create_2d9, create_2a6, create_292, create_23b
+from selfdrive.car.chrysler.values import ECU, STATIC_MSGS
+from selfdrive.can.packer import CANPacker
+
+# Steer torque range is 1024+-230. The 1024 is added by our library.
+# degrees * 3.0 = car units
+CAR_UNITS_PER_DEGREE = 3.0  # originally 5.1
+STEER_MAX = 230 / CAR_UNITS_PER_DEGREE  # degrees
+STEER_DELTA_UP = 2.5 / CAR_UNITS_PER_DEGREE  # degrees
+STEER_DELTA_DOWN = 2.5 / CAR_UNITS_PER_DEGREE  # degrees
+MIN_STEER_MS = 3.8  # TODO consolidate with interface.py CP.minSteerSpeed.
+# 3.0 works, but increased to 3.8 to make it more reliable.
+
+
+class CarController(object):
+  def __init__(self, dbc_name, car_fingerprint, enable_camera):
+    self.braking = False
+    # redundant safety check with the board
+    self.controls_allowed = True
+    self.last_steer = 0
+    self.prev_frame = -1  # previous frame from interface from 220 frame
+    self.ccframe = 0
+    self.car_fingerprint = car_fingerprint
+    self.alert_active = False
+
+    self.fake_ecus = set()
+    if enable_camera: self.fake_ecus.add(ECU.CAM)
+
+    self.packer = CANPacker(dbc_name)
+
+
+  def update(self, sendcan, enabled, CS, frame, actuators,
+             pcm_cancel_cmd, hud_alert, audible_alert, send_cancel_acc):
+
+    # *** compute control surfaces ***
+    # steer torque
+    apply_steer = int(round(actuators.steer * STEER_MAX))
+    apply_steer = clip(apply_steer, -STEER_MAX, STEER_MAX)
+
+    # slow rate if steer torque increases in magnitude
+    if self.last_steer > 0:
+      apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP)
+    else:
+      apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
+
+    moving_fast = True  # for status message
+    if CS.v_ego < MIN_STEER_MS:  # don't steer if going slow to not lock out LKAS
+      apply_steer = 0
+      moving_fast = False
+
+    self.last_steer = apply_steer
+
+    if self.prev_frame == frame:
+      return  # Do not reuse an old frame. This avoids repeating on shut-down.
+
+    can_sends = []
+
+    #*** control msgs ***
+
+    if send_cancel_acc:
+      new_msg = create_23b(CS.frame_23b + 2)
+      sendcan.send(can_list_to_can_capnp([new_msg], msgtype='sendcan').to_bytes())
+
+    # frame is 100Hz (0.01s period)
+    if (self.ccframe % 10 == 0):  # 0.1s period
+      new_msg = create_2d9(self.car_fingerprint)
+      can_sends.append(new_msg)
+    if (self.ccframe % 25 == 0):  # 0.25s period
+      new_msg = create_2a6(CS.gear_shifter, apply_steer, moving_fast, self.car_fingerprint)
+      can_sends.append(new_msg)
+    new_msg = create_292(int(apply_steer * CAR_UNITS_PER_DEGREE), frame)
+    self.prev_frame = frame  # save so we do not reuse frames
+    can_sends.append(new_msg)  # degrees * 5.1 -> car steering units
+
+    self.ccframe += 1
+    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())

selfdrive/car/chrysler/carstate.py

@@ -0,0 +1,140 @@
+import logging
+from selfdrive.can.parser import CANParser
+from selfdrive.config import Conversions as CV
+from selfdrive.car.chrysler.values import CAR, DBC
+from common.kalman.simple_kalman import KF1D
+import numpy as np
+
+
+def parse_gear_shifter(can_gear):
+  if can_gear == 0x1:
+    return "park"
+  elif can_gear == 0x2:
+    return "reverse"
+  elif can_gear == 0x3:
+    return "neutral"
+  elif can_gear == 0x4:
+    return "drive"
+  return "unknown"
+
+
+def get_can_parser(CP):
+
+  signals = [
+    # sig_name, sig_address, default
+    ("PRNDL", "GEAR", 0),
+    ("DOOR_OPEN_FL", "DOORS", 0),
+    ("DOOR_OPEN_FR", "DOORS", 0),
+    ("DOOR_OPEN_RL", "DOORS", 0),
+    ("DOOR_OPEN_RR", "DOORS", 0),
+    ("BRAKE_PRESSED_2", "BRAKE_2", 0),
+    ("ACCEL_PEDAL", "ACCEL_PEDAL_MSG", 0),
+    ("SPEED_LEFT", "SPEED_1", 0),
+    ("SPEED_RIGHT", "SPEED_1", 0),
+    ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
+    ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
+    ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
+    ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
+    ("STEER_ANGLE", "STEERING", 0),
+    ("STEERING_RATE", "STEERING", 0),
+    ("TURN_SIGNALS", "STEERING_LEVERS", 0),
+    ("ACC_STATUS_2", "ACC_2", 0),
+    ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0),
+    ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0),
+    ("INCREMENTING_220", "LKAS_INDICATOR_1", -1),
+    ("LKAS_IS_GREEN", "LKAS_INDICATOR_1", 1),
+    ("TRACTION_OFF", "TRACTION_BUTTON", 0),
+    ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0),
+    ("COUNTER", "WHEEL_BUTTONS", -1),  # incrementing counter for 23b
+  ]
+
+  # It's considered invalid if it is not received for 10x the expected period (1/f).
+  checks = [
+    # sig_address, frequency
+    ("BRAKE_2", 50),
+    ("LKAS_INDICATOR_1", 100),
+    ("SPEED_1", 100),
+    ("WHEEL_SPEEDS", 50),
+    ("STEERING", 100),
+    ("ACC_2", 50),
+  ]
+
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+
+
+class CarState(object):
+  def __init__(self, CP):
+
+    self.CP = CP
+    self.left_blinker_on = 0
+    self.right_blinker_on = 0
+
+    # initialize can parser
+    self.car_fingerprint = CP.carFingerprint
+
+    # vEgo kalman filter
+    dt = 0.01
+    # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
+    # R = 1e3
+    self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
+                         A=np.matrix([[1.0, dt], [0.0, 1.0]]),
+                         C=np.matrix([1.0, 0.0]),
+                         K=np.matrix([[0.12287673], [0.29666309]]))
+    self.v_ego = 0.0
+
+
+  def update(self, cp):
+    # copy can_valid
+    self.can_valid = cp.can_valid
+
+    # update prevs, update must run once per loop
+    self.prev_left_blinker_on = self.left_blinker_on
+    self.prev_right_blinker_on = self.right_blinker_on
+
+    self.frame_220 = int(cp.vl["LKAS_INDICATOR_1"]['INCREMENTING_220'])
+    self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER'])
+
+    self.door_all_closed = not any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
+                                    cp.vl["DOORS"]['DOOR_OPEN_FR'],
+                                    cp.vl["DOORS"]['DOOR_OPEN_RL'],
+                                    cp.vl["DOORS"]['DOOR_OPEN_RR']])
+    self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
+
+    self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
+    self.pedal_gas = cp.vl["ACCEL_PEDAL_MSG"]['ACCEL_PEDAL']
+    self.car_gas = self.pedal_gas
+    self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
+
+    self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
+    self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
+    self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
+    self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
+    self.v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
+
+    # Kalman filter
+    if abs(self.v_wheel - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
+      self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+
+    self.v_ego_raw = self.v_wheel
+    v_ego_x = self.v_ego_kf.update(self.v_wheel)
+    self.v_ego = float(v_ego_x[0])
+    self.a_ego = float(v_ego_x[1])
+    self.standstill = not self.v_wheel > 0.001
+
+    self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
+    self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
+    self.gear_shifter = parse_gear_shifter(cp.vl['GEAR']['PRNDL'])
+    self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7  # ACC is green.
+    self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
+    self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
+
+    self.steer_override = False  # TODO
+    self.steer_error = cp.vl["LKAS_INDICATOR_1"]['LKAS_IS_GREEN'] == 0  # 0 if wheel will not actuate
+    self.steer_torque_driver = 0  # TODO
+
+    self.user_brake = 0
+    self.brake_lights = self.brake_pressed
+    self.v_cruise_pcm = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH']
+    self.pcm_acc_status = self.main_on
+
+    self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])