diff --git a/selfdrive/car/nissan/__init__.py b/selfdrive/car/nissan/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py new file mode 100644 index 0000000000..f79617f560 --- /dev/null +++ b/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 diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py new file mode 100644 index 0000000000..5ce06955ad --- /dev/null +++ b/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) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py new file mode 100644 index 0000000000..caf8456951 --- /dev/null +++ b/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 diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py new file mode 100644 index 0000000000..9e42e0fe40 --- /dev/null +++ b/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) diff --git a/selfdrive/car/nissan/radar_interface.py b/selfdrive/car/nissan/radar_interface.py new file mode 100644 index 0000000000..b2f7651136 --- /dev/null +++ b/selfdrive/car/nissan/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py new file mode 100644 index 0000000000..c9b30585b0 --- /dev/null +++ b/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), +} diff --git a/selfdrive/test/process_replay/model_ref_commit b/selfdrive/test/process_replay/model_ref_commit index 7e0a58e2b0..c44a660c4e 100644 --- a/selfdrive/test/process_replay/model_ref_commit +++ b/selfdrive/test/process_replay/model_ref_commit @@ -1 +1 @@ -95638846316e5de7f0314ed2330b01428792c889 \ No newline at end of file +6829c5c76f3527af06e1c2b685f98a5e1bbef00a \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 54eae7c27d..a6e8d68a5b 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/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/" diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index e9960a7e98..0fb5ed94f8 100755 --- a/selfdrive/test/test_car_models.py +++ b/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