Merge pull request commaai#124 from arne182/chrysler

Chrysler

panda/board/safety/safety_chrysler.h

@@ -1,8 +1,8 @@
 const int CHRYSLER_MAX_STEER = 261;
-const int CHRYSLER_MAX_RT_DELTA = 112;        // max delta torque allowed for real time checks
+const int CHRYSLER_MAX_RT_DELTA = 112;        // max delta torque allowed for real time checks.
 const int32_t CHRYSLER_RT_INTERVAL = 250000;  // 250ms between real time checks
-const int CHRYSLER_MAX_RATE_UP = 3;
-const int CHRYSLER_MAX_RATE_DOWN = 3;
+const int CHRYSLER_MAX_RATE_UP = 3 * 10;     // do not want to strictly enforce in case we miss a message or two.
+const int CHRYSLER_MAX_RATE_DOWN = 3 * 10;
 const int CHRYSLER_MAX_TORQUE_ERROR = 80;    // max torque cmd in excess of torque motor
 
 int chrysler_camera_detected = 0;
@@ -82,14 +82,19 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       violation |= max_limit_check(desired_torque, CHRYSLER_MAX_STEER, -CHRYSLER_MAX_STEER);
 
       // *** torque rate limit check ***
-      violation |= dist_to_meas_check(desired_torque, chrysler_desired_torque_last,
-        &chrysler_torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR);
-
+      // This triggers on drives if the driver moves the wheel a lot.
+      // TODO Figure out how to add this back in. For now using a simpler rate check.
+      // violation |= dist_to_meas_check(desired_torque, chrysler_desired_torque_last,
+      //     &chrysler_torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR);
+      violation |= (desired_torque < (chrysler_desired_torque_last - CHRYSLER_MAX_RATE_DOWN));
+      violation |= (desired_torque > (chrysler_desired_torque_last + CHRYSLER_MAX_RATE_UP));
+
       // used next time
       chrysler_desired_torque_last = desired_torque;
 
       // *** torque real time rate limit check ***
-      violation |= rt_rate_limit_check(desired_torque, chrysler_rt_torque_last, CHRYSLER_MAX_RT_DELTA);
+      // TODO This triggers on some drives. Figure out how to add it back in.
+      // violation |= rt_rate_limit_check(desired_torque, chrysler_rt_torque_last, CHRYSLER_MAX_RT_DELTA);
 
       // every RT_INTERVAL set the new limits
       uint32_t ts_elapsed = get_ts_elapsed(ts, chrysler_ts_last);
@@ -125,12 +130,22 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return true;
 }
 
+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
+  // TODO not sure if we need to filter out LKAS addrs, they should be bus 128.
+  if (bus_num == 0 && addr != 0x2d9 && addr != 0x2a6 && addr != 0x292) {
+    return 2;
+  }
+  return -1;  // do not forward
+}
+
 
 const safety_hooks chrysler_hooks = {
   .init = nooutput_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,
 };

panda/tests/safety/test_chrysler.py

@@ -1,10 +1,12 @@
 #!/usr/bin/env python2
+import csv
+import glob
 import unittest
 import numpy as np
 import libpandasafety_py
 
-MAX_RATE_UP = 3
-MAX_RATE_DOWN = 3
+MAX_RATE_UP = 3 * 10  # do not want to strictly enforce
+MAX_RATE_DOWN = 3 * 10
 MAX_STEER = 261
 
 MAX_RT_DELTA = 112
@@ -24,6 +26,11 @@ def sign(a):
   else:
     return -1
 
+def swap_bytes(data_str):
+  """Accepts string with hex, returns integer with order swapped for CAN."""
+  a = int(data_str, 16)
+  return ((a & 0xff) << 24) + ((a & 0xff00) << 8) + ((a & 0x00ff0000) >> 8) + ((a & 0xff000000) >> 24)
+
 class TestChryslerSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
@@ -111,7 +118,7 @@ def test_non_realtime_limit_down(self):
     self.safety.set_chrysler_rt_torque_last(MAX_STEER)
     self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
     self.safety.set_chrysler_desired_torque_last(MAX_STEER)
-    self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1)))
+    self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN - 1)))
 
   def test_exceed_torque_sensor(self):
     self.safety.set_controls_allowed(True)
@@ -122,7 +129,8 @@ def test_exceed_torque_sensor(self):
         t *= sign
         self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
 
-      self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2))))
+      # torque_meas check is currently disabld, so diable the test:
+      # self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2))))
 
   def test_realtime_limit_up(self):
     self.safety.set_controls_allowed(True)
@@ -134,7 +142,8 @@ def test_realtime_limit_up(self):
         t *= sign
         self.safety.set_chrysler_torque_meas(t, t)
         self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
-      self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
+      # MAX_RT_DELTA check is currently disabled, so disable the test:
+      # self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
 
       self._set_prev_torque(0)
       for t in np.arange(0, MAX_RT_DELTA+1, 1):
@@ -166,6 +175,38 @@ def test_torque_measurements(self):
     self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
     self.assertEqual(0, self.safety.get_chrysler_torque_meas_min())
 
+  def _replay_drive(self, csv_reader):
+    error_count = 0  # errors in a row, 1 or 2 is fine for timing.
+    for row in csv_reader:
+      if len(row) != 4:  # sometimes truncated at end of the file
+        continue
+      if row[0] == 'time':  # skip CSV header
+        continue
+      addr = int(row[1])
+      bus = int(row[2])
+      data_str = row[3]  # Example '081407ff0806e06f'
+      to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
+      to_send[0].RIR = addr << 21
+      to_send[0].RDHR = swap_bytes(data_str[8:])
+      to_send[0].RDLR = swap_bytes(data_str[:8])
+      if (bus == 128):
+        if not self.safety.chrysler_tx_hook(to_send):
+          error_count += 1
+        else:
+          error_count = 0
+        self.assertTrue(error_count < 2, msg=row)
+      else:
+        self.safety.chrysler_rx_hook(to_send)
+
+  def test_replay_drive(self):
+    # In Cabana, click "Save Log" and then put the downloaded CSV in this directory.
+    test_files = glob.glob('chrysler_*.csv')
+    for filename in test_files:
+      print 'testing %s' % filename
+      with open(filename) as csvfile:
+        reader = csv.reader(csvfile)
+        self._replay_drive(reader)
+
 
 if __name__ == "__main__":
   unittest.main()

selfdrive/car/chrysler/carcontroller.py

@@ -1,10 +1,12 @@
 from cereal import car
+from common.numpy_fast import interp
 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_chimes
-from selfdrive.car.chrysler.values import ECU
+from selfdrive.car.modules.ALCA_module import ALCAController
+from selfdrive.car.chrysler.values import ECU, CAR
 from selfdrive.can.packer import CANPacker
 
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
@@ -29,7 +31,10 @@ 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.ALCA = ALCAController(self,True,False)  # Enabled  True and SteerByAngle only False
+
     self.fake_ecus = set()
     if enable_camera:
       self.fake_ecus.add(ECU.CAM)
@@ -39,25 +44,52 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera):
 
   def update(self, sendcan, enabled, CS, frame, actuators,
              pcm_cancel_cmd, hud_alert, audible_alert):
-
+    #update custom UI buttons and alerts
+    CS.UE.update_custom_ui()
+    if (frame % 1000 == 0):
+      CS.cstm_btns.send_button_info()
+      CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name)
+
+    # Get the angle from ALCA.
+    alca_enabled = False
+    alca_steer = 0.
+    alca_angle = 0.
+    turn_signal_needed = 0
+    # Update ALCA status and custom button every 0.1 sec.
+    if self.ALCA.pid == None:
+      self.ALCA.set_pid(CS)
+    if (frame % 10 == 0):
+      self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
+    # steer torque
+    alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators)
+
     # 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
 
     # *** compute control surfaces ***
     # steer torque
-    apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
+    apply_steer = alca_steer * SteerLimitParams.STEER_MAX
     apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.apply_steer_last,
                                                    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:
       apply_steer = 0
-
+
+    if CS.cstm_btns.get_button_status("lka") == 0:
+        apply_steer = 0
     self.apply_steer_last = apply_steer
-
+
+
     if audible_alert in LOUD_ALERTS:
       self.send_chime = True
 
@@ -78,16 +110,18 @@ def update(self, sendcan, enabled, CS, frame, actuators,
 
     # frame is 100Hz (0.01s period)
     if (self.ccframe % 10 == 0):  # 0.1s period
-      new_msg = create_lkas_heartbit(self.car_fingerprint)
+      new_msg = create_lkas_heartbit(self.packer, CS.lkas_status_ok)
       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

@@ -1,6 +1,8 @@
 from selfdrive.can.parser import CANParser
 from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
 from common.kalman.simple_kalman import KF1D
+from selfdrive.car.modules.UIBT_module import UIButtons,UIButton
+from selfdrive.car.modules.UIEV_module import UIEvents
 import numpy as np
 
 
@@ -63,11 +65,79 @@ 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):
-
+    self.alcaLabels = ["MadMax","Normal","Wifey"]
+    self.alcaMode = 2     # default to wifey on startup
+    self.prev_distance_button = 0
+    self.distance_button = 0
+    self.prev_lka_button = 0
+    self.lka_button = 0
+    self.lkMode = True
+
+    # ALCA PARAMS
+    self.blind_spot_on = bool(0)
+    # max REAL delta angle for correction vs actuator
+    self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 44.]
+    self.CL_MAX_ANGLE_DELTA = [0.77, 0.86, 0.53]
+    # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
+    self.CL_ADJUST_FACTOR_BP = [10., 44.]
+    self.CL_ADJUST_FACTOR = [16. , 8.]
+    # reenrey angle when to let go
+    self.CL_REENTRY_ANGLE_BP = [10., 44.]
+    self.CL_REENTRY_ANGLE = [5. , 5.]
+    # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
+    self.CL_LANE_DETECT_BP = [10., 44.]
+    self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
+    self.CL_LANE_PASS_BP = [10., 20., 44.]
+    self.CL_LANE_PASS_TIME = [40.,10., 4.] 
+    # change lane delta angles and other params
+    self.CL_MAXD_BP = [10., 32., 44.]
+    self.CL_MAXD_A = [.358, 0.084, 0.040] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
+    self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
+    # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
+    self.CL_MAX_A_BP = [10., 44.]
+    self.CL_MAX_A = [10., 10.] 
+    # define limits for angle change every 0.1 s
+    # we need to force correction above 10 deg but less than 20
+    # anything more means we are going to steep or not enough in a turn
+    self.CL_MAX_ACTUATOR_DELTA = 2.
+    self.CL_MIN_ACTUATOR_DELTA = 0. 
+    self.CL_CORRECTION_FACTOR = [1.,1.1,1.2]
+    self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
+    #duration after we cross the line until we release is a factor of speed
+    self.CL_TIMEA_BP = [10., 32., 44.]
+    self.CL_TIMEA_T = [0.2 ,0.2, 0.2]
+    #duration to wait (in seconds) with blinkers on before starting to turn
+    self.CL_WAIT_BEFORE_START = 1
+    #END OF ALCA PARAMS
+
     self.CP = CP
+
+    #BB UIEvents
+    self.UE = UIEvents(self)
+
+   #BB variable for custom buttons
+    self.cstm_btns = UIButtons(self,"Chrysler","chrysler")
+
+    #BB pid holder for ALCA
+    self.pid = None
+
+    #BB custom message counter
+    self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
+
     self.left_blinker_on = 0
     self.right_blinker_on = 0
 
@@ -83,9 +153,33 @@ def __init__(self, CP):
                          C=np.matrix([1.0, 0.0]),
                          K=np.matrix([[0.12287673], [0.29666309]]))
     self.v_ego = 0.0
-
-
-  def update(self, cp):
+
+   #BB init ui buttons
+  def init_ui_buttons(self):
+    btns = []
+    btns.append(UIButton("sound", "SND", 0, "", 0))
+    btns.append(UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 1))
+    btns.append(UIButton("","",0,"",2))
+    btns.append(UIButton("","",0,"",3))
+    btns.append(UIButton("gas","GAS",1,"",4))
+    btns.append(UIButton("lka","LKA",1,"",5))
+    return btns
+  #BB update ui buttons
+  def update_ui_buttons(self,id,btn_status):
+    if self.cstm_btns.btns[id].btn_status > 0:
+      if (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca":
+          if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]:
+            self.alcaMode = (self.alcaMode + 1 ) % 3
+          else:
+            self.alcaMode = 0
+          self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode]
+          self.cstm_btns.hasChanges = True
+      else:
+        self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status
+    else:
+        self.cstm_btns.btns[id].btn_status = btn_status
+
+  def update(self, cp, cp_cam):
     # copy can_valid
     self.can_valid = cp.can_valid
 
@@ -142,3 +236,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']