Nissan port - dashcam only (#1203)

* Nissan port

* add x-trail to release notes

* update readme and model

* remove from releases until tested

* Don't test Nissan for now

* Forced dashcam in test_car_models

selfdrive/car/nissan/carcontroller.py

@@ -0,0 +1,73 @@
+from common.numpy_fast import clip, interp
+from selfdrive.car.nissan import nissancan
+from opendbc.can.packer import CANPacker
+
+# Steer angle limits
+ANGLE_MAX_BP = [1.3, 10., 30.]
+ANGLE_MAX_V = [540., 120., 23.]
+ANGLE_DELTA_BP = [0., 5., 15.]
+ANGLE_DELTA_V = [5., .8, .15]     # windup limit
+ANGLE_DELTA_VU = [5., 3.5, 0.4]   # unwind limit
+LKAS_MAX_TORQUE = 100             # A value of 100 is easy to overpower
+
+
+class CarController():
+  def __init__(self, dbc_name, CP, VM):
+    self.car_fingerprint = CP.carFingerprint
+
+    self.lkas_max_torque = 0
+    self.last_angle = 0
+
+    self.packer = CANPacker(dbc_name)
+
+  def update(self, enabled, CS, frame, actuators, cruise_cancel):
+    """ Controls thread """
+
+    # Send CAN commands.
+    can_sends = []
+
+    ### STEER ###
+    acc_active = bool(CS.out.cruiseState.enabled)
+    cruise_throttle_msg = CS.cruise_throttle_msg
+    apply_angle = actuators.steerAngle
+
+    if enabled:
+      # # windup slower
+      if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
+        angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V)
+      else:
+        angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
+
+      apply_angle = clip(apply_angle, self.last_angle -
+                         angle_rate_lim, self.last_angle + angle_rate_lim)
+
+      # steer angle
+      angle_lim = interp(CS.out.vEgo, ANGLE_MAX_BP, ANGLE_MAX_V)
+      apply_angle = clip(apply_angle, -angle_lim, angle_lim)
+
+      # Max torque from driver before EPS will give up and not apply torque
+      if not bool(CS.out.steeringPressed):
+        self.lkas_max_torque = LKAS_MAX_TORQUE
+      else:
+        # Scale max torque based on how much torque the driver is applying to the wheel
+        self.lkas_max_torque = max(
+            0, LKAS_MAX_TORQUE - 0.4 * abs(CS.out.steeringTorque))
+
+    else:
+      apply_angle = CS.out.steeringAngle
+      self.lkas_max_torque = 0
+
+    self.last_angle = apply_angle
+
+    if not enabled and acc_active:
+      # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
+      cruise_cancel = 1
+
+    if cruise_cancel:
+      can_sends.append(nissancan.create_acc_cancel_cmd(
+          self.packer, cruise_throttle_msg, frame))
+
+    can_sends.append(nissancan.create_steering_control(
+        self.packer, self.car_fingerprint, apply_angle, frame, acc_active, self.lkas_max_torque))
+
+    return can_sends

selfdrive/car/nissan/carstate.py

@@ -0,0 +1,137 @@
+import copy
+from cereal import car
+from opendbc.can.can_define import CANDefine
+from selfdrive.car.interfaces import CarStateBase
+from selfdrive.config import Conversions as CV
+from opendbc.can.parser import CANParser
+from selfdrive.car.nissan.values import DBC
+
+
+class CarState(CarStateBase):
+  def __init__(self, CP):
+    super().__init__(CP)
+    can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
+    self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
+
+
+  def update(self, cp, cp_adas, cp_cam):
+    ret = car.CarState.new_message()
+
+    ret.gas = cp.vl["Throttle"]["ThrottlePedal"]
+    ret.gasPressed = bool(ret.gas)
+    ret.brakePressed = bool(cp.vl["DoorsLights"]["USER_BRAKE_PRESSED"])
+    ret.brakeLights = bool(cp.vl["DoorsLights"]["BRAKE_LIGHT"])
+
+    ret.wheelSpeeds.fl = cp.vl["WheelspeedFront"]["FL"] * CV.KPH_TO_MS
+    ret.wheelSpeeds.fr = cp.vl["WheelspeedFront"]["FR"] * CV.KPH_TO_MS
+    ret.wheelSpeeds.rl = cp.vl["WheelspeedRear"]["RL"] * CV.KPH_TO_MS
+    ret.wheelSpeeds.rr = cp.vl["WheelspeedRear"]["RR"] * CV.KPH_TO_MS
+
+    ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
+
+    # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+    ret.standstill = ret.vEgoRaw < 0.01
+
+    can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"])
+    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
+
+    ret.leftBlinker = bool(cp.vl["Lights"]["LEFT_BLINKER"])
+    ret.rightBlinker = bool(cp.vl["Lights"]["RIGHT_BLINKER"])
+
+    ret.seatbeltUnlatched = cp.vl["_SEATBELT"]["DRIVERS_SEATBELT"] == 0
+    ret.cruiseState.enabled = bool(cp_cam.vl["ProPilot"]["CRUISE_ACTIVATED"])
+    ret.cruiseState.available = bool(cp_cam.vl["ProPilot"]["CRUISE_ON"])
+
+    ret.doorOpen = any([cp.vl["DoorsLights"]["DOOR_OPEN_RR"],
+      cp.vl["DoorsLights"]["DOOR_OPEN_RL"],
+      cp.vl["DoorsLights"]["DOOR_OPEN_FR"],
+      cp.vl["DoorsLights"]["DOOR_OPEN_FL"]])
+
+    ret.steeringPressed = bool(cp.vl["STEER_TORQUE"]["DriverTouchingWheel"])
+    ret.steeringTorque = cp.vl["Steering"]["DriverTorque"]
+    ret.steeringAngle = cp.vl["SteeringWheel"]["Steering_Angle"]
+
+    ret.espDisabled = bool(cp.vl["_ESP"]["ESP_DISABLED"])
+
+    self.cruise_throttle_msg = copy.copy(cp.vl["CruiseThrottle"])
+
+    return ret
+
+  @staticmethod
+  def get_can_parser(CP):
+    # this function generates lists for signal, messages and initial values
+    signals = [
+      # sig_name, sig_address, default
+      ("FL", "WheelspeedFront", 0),
+      ("FR", "WheelspeedFront", 0),
+      ("RL", "WheelspeedRear", 0),
+      ("RR", "WheelspeedRear", 0),
+      ("DOOR_OPEN_FR", "DoorsLights", 1),
+      ("DOOR_OPEN_FL", "DoorsLights", 1),
+      ("DOOR_OPEN_RR", "DoorsLights", 1),
+      ("DOOR_OPEN_RL", "DoorsLights", 1),
+      ("USER_BRAKE_PRESSED", "DoorsLights", 1),
+      ("BRAKE_LIGHT", "DoorsLights", 1),
+      ("DriverTorque", "Steering", 0),
+      ("DriverTouchingWheel", "STEER_TORQUE", 0),
+      ("ThrottlePedal", "Throttle", 0),
+      ("Steering_Angle", "SteeringWheel", 0),
+      ("RIGHT_BLINKER", "Lights", 0),
+      ("LEFT_BLINKER", "Lights", 0),
+      ("PROPILOT_BUTTON", "CruiseThrottle", 0),
+      ("CANCEL_BUTTON", "CruiseThrottle", 0),
+      ("GAS_PEDAL_INVERTED", "CruiseThrottle", 0),
+      ("unsure2", "CruiseThrottle", 0),
+      ("SET_BUTTON", "CruiseThrottle", 0),
+      ("RES_BUTTON", "CruiseThrottle", 0),
+      ("FOLLOW_DISTANCE_BUTTON", "CruiseThrottle", 0),
+      ("NO_BUTTON_PRESSED", "CruiseThrottle", 0),
+      ("GAS_PEDAL", "CruiseThrottle", 0),
+      ("unsure3", "CruiseThrottle", 0),
+      ("unsure", "CruiseThrottle", 0),
+      ("DRIVERS_SEATBELT", "_SEATBELT", 0),
+      ("ESP_DISABLED", "_ESP", 0),
+      ("GEAR_SHIFTER", "GEARBOX", 0),
+    ]
+
+    checks = [
+      # sig_address, frequency
+      ("WheelspeedRear", 50),
+      ("WheelspeedFront", 50),
+      ("DoorsLights", 10),
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+
+  @staticmethod
+  def get_adas_can_parser(CP):
+    # this function generates lists for signal, messages and initial values
+    signals = [
+      # sig_name, sig_address, default
+      ("DESIRED_ANGLE", "LKAS", 0),
+      ("SET_0x80_2", "LKAS", 0),
+      ("MAX_TORQUE", "LKAS", 0),
+      ("SET_0x80", "LKAS", 0),
+      ("COUNTER", "LKAS", 0),
+      ("LKA_ACTIVE", "LKAS", 0),
+    ]
+
+    checks = [
+      # sig_address, frequency
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
+
+  @staticmethod
+  def get_cam_can_parser(CP):
+    signals = [
+      ("CRUISE_ON", "ProPilot", 0),
+      ("CRUISE_ACTIVATED", "ProPilot", 0),
+      ("STEER_STATUS", "ProPilot", 0),
+    ]
+
+    checks = [
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)

selfdrive/car/nissan/interface.py

@@ -0,0 +1,120 @@
+#!/usr/bin/env python3
+from cereal import car
+from selfdrive.config import Conversions as CV
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
+from selfdrive.car.nissan.values import CAR
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car.interfaces import CarInterfaceBase
+
+class CarInterface(CarInterfaceBase):
+  def __init__(self, CP, CarController, CarState):
+    super().__init__(CP, CarController, CarState)
+    self.cp_adas = self.CS.get_adas_can_parser(CP)
+
+  @staticmethod
+  def compute_gb(accel, speed):
+    return float(accel) / 4.0
+
+  @staticmethod
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
+
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret.dashcamOnly = True
+
+    ret.carName = "nissan"
+    ret.carFingerprint = candidate
+    ret.isPandaBlack = has_relay
+    ret.safetyModel = car.CarParams.SafetyModel.nissan
+
+    ret.steerLimitAlert = False
+
+    ret.enableCamera = True
+
+    ret.steerRateCost = 0.5
+
+    if candidate in [CAR.XTRAIL]:
+      ret.mass = 1610 + STD_CARGO_KG
+      ret.wheelbase = 2.705
+      ret.centerToFront = ret.wheelbase * 0.44
+      ret.steerRatio = 17
+      ret.steerActuatorDelay = 0.1
+      ret.lateralTuning.pid.kf = 0.00006
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.0], [0.0]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]]
+      ret.steerMaxBP = [0.] # m/s
+      ret.steerMaxV = [1.]
+
+    ret.steerControlType = car.CarParams.SteerControlType.angle
+    ret.steerRatioRear = 0.
+    # testing tuning
+
+    # No long control in nissan
+    ret.gasMaxBP = [0.]
+    ret.gasMaxV = [0.]
+    ret.brakeMaxBP = [0.]
+    ret.brakeMaxV = [0.]
+    ret.longitudinalTuning.deadzoneBP = [0.]
+    ret.longitudinalTuning.deadzoneV = [0.]
+    ret.longitudinalTuning.kpBP = [0.]
+    ret.longitudinalTuning.kpV = [0.]
+    ret.longitudinalTuning.kiBP = [0.]
+    ret.longitudinalTuning.kiV = [0.]
+
+    ret.radarOffCan = True
+
+    # TODO: get actual value, for now starting with reasonable value for
+    # civic and scaling by mass and wheelbase
+    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+
+    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
+    # mass and CG position, so all cars will have approximately similar dyn behaviors
+    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
+
+    return ret
+
+  # returns a car.CarState
+  def update(self, c, can_strings):
+    self.cp.update_strings(can_strings)
+    self.cp_cam.update_strings(can_strings)
+    self.cp_adas.update_strings(can_strings)
+
+    ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
+
+    ret.canValid = self.cp.can_valid and self.cp_adas.can_valid and self.cp_cam.can_valid
+    ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
+
+    buttonEvents = []
+    be = car.CarState.ButtonEvent.new_message()
+    be.type = car.CarState.ButtonEvent.Type.accelCruise
+    buttonEvents.append(be)
+
+    events = self.create_common_events(ret)
+
+    if ret.cruiseState.enabled and not self.cruise_enabled_prev:
+      events.append(create_event('pcmEnable', [ET.ENABLE]))
+    if not ret.cruiseState.enabled:
+      events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
+
+    # disable on pedals rising edge or when brake is pressed and speed isn't zero
+    if (ret.gasPressed and not self.gas_pressed_prev) or \
+       (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
+      events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+
+    if ret.gasPressed:
+      events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+
+    ret.events = events
+
+    # update previous brake/gas pressed
+    self.gas_pressed_prev = ret.gasPressed
+    self.brake_pressed_prev = ret.brakePressed
+    self.cruise_enabled_prev = ret.cruiseState.enabled
+
+    self.CS.out = ret.as_reader()
+    return self.CS.out
+
+  def apply(self, c):
+    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
+                               c.cruiseControl.cancel,)
+    self.frame += 1
+    return can_sends

selfdrive/car/nissan/nissancan.py

@@ -0,0 +1,38 @@
+import copy
+import crcmod
+from selfdrive.car.nissan.values import CAR
+
+nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
+
+
+def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_on, lkas_max_torque):
+  if car_fingerprint == CAR.XTRAIL:
+    idx = (frame % 16)
+    values = {
+      "DESIRED_ANGLE": apply_steer,
+      "SET_0x80_2": 0x80,
+      "SET_0x80": 0x80,
+      "MAX_TORQUE": lkas_max_torque if steer_on else 0,
+      "COUNTER": idx,
+      "LKA_ACTIVE": steer_on,
+    }
+
+    dat = packer.make_can_msg("LKAS", 0, values)[2]
+
+    values["CRC"] = nissan_checksum(dat[:7])
+
+  return packer.make_can_msg("LKAS", 0, values)
+
+
+def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame):
+  values = copy.copy(cruise_throttle_msg)
+
+  values["CANCEL_BUTTON"] = 1
+  values["NO_BUTTON_PRESSED"] = 0
+  values["PROPILOT_BUTTON"] = 0
+  values["SET_BUTTON"] = 0
+  values["RES_BUTTON"] = 0
+  values["FOLLOW_DISTANCE_BUTTON"] = 0
+  values["COUNTER"] = (frame % 4)
+  
+  return packer.make_can_msg("CruiseThrottle", 2, values)

selfdrive/car/nissan/radar_interface.py

@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+  pass

selfdrive/car/nissan/values.py

@@ -0,0 +1,20 @@
+from selfdrive.car import dbc_dict
+
+class CAR:
+  XTRAIL = "NISSAN X-TRAIL 2017"
+
+FINGERPRINTS = {
+  CAR.XTRAIL: [{
+    2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 2015: 8, 2016: 8, 2024: 8
+  }, {
+    2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 723: 8, 758: 3, 768: 2, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1266: 8, 1273: 7, 1376: 6, 1497: 3
+  }, {
+    2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3,768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837:8, 2015: 8, 2024: 8
+  }, {
+    2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3,1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8
+  }],
+}
+
+DBC = {
+  CAR.XTRAIL: dbc_dict('nissan_2017', None),
+}

selfdrive/test/process_replay/model_ref_commit

@@ -1 +1 @@
-95638846316e5de7f0314ed2330b01428792c889
+6829c5c76f3527af06e1c2b685f98a5e1bbef00a

selfdrive/test/process_replay/test_processes.py

@@ -22,11 +22,14 @@ segments = [
   ("HYUNDAI", "38bfd238edecbcd7|2018-08-29--22-02-15--4"),    # HYUNDAI.SANTA_FE
   #("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"),   # CHRYSLER.JEEP_CHEROKEE
   ("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"),     # SUBARU.IMPREZA
-  ("VOLKSWAGEN", "b0c9d2329ad1606b|2020-02-19--16-29-36--7"),  # VW.GOLF
+  ("VOLKSWAGEN", "b0c9d2329ad1606b|2020-02-19--16-29-36--7"), # VW.GOLF
+
+  # Enable when port is tested and dascamOnly is no longer set
+  # ("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"),     # NISSAN.XTRAIL
 ]
 
 # ford doesn't need to be tested until a full port is done
-excluded_interfaces = ["mock", "ford"]
+excluded_interfaces = ["mock", "ford", "nissan"]
 
 BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
 

selfdrive/test/test_car_models.py

@@ -20,6 +20,7 @@ from selfdrive.car.hyundai.values import CAR as HYUNDAI
 from selfdrive.car.chrysler.values import CAR as CHRYSLER
 from selfdrive.car.subaru.values import CAR as SUBARU
 from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN
+from selfdrive.car.nissan.values import CAR as NISSAN
 from selfdrive.car.mock.values import CAR as MOCK
 
 
@@ -434,6 +435,10 @@ routes = {
   #"bfa17080b080f3ec|2018-06-28--23-27-47": {
   #  'carFingerprint': MOCK.MOCK,
   #},
+  "fbbfa6af821552b9|2020-03-03--08-09-43": {
+    'carFingerprint': NISSAN.XTRAIL,
+    'enableCamera': True,
+  },
 }
 
 passive_routes = [
@@ -445,6 +450,9 @@ forced_dashcam_routes = [
   # Ford fusion
   "f1b4c567731f4a1b|2018-04-18--11-29-37",
   "f1b4c567731f4a1b|2018-04-30--10-15-35",
+
+  # Nissan
+  "fbbfa6af821552b9|2020-03-03--08-09-43",
 ]
 
 # TODO: replace all these with public routes