diff --git a/cereal b/cereal index b0c746b1e1..f515e4db2e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit b0c746b1e1e3ca04f61484fd073123a1bf96afd1 +Subproject commit f515e4db2e9941158d1c477f35b186df5633aa2e diff --git a/opendbc b/opendbc index 50fbbe7396..a4407d6049 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 50fbbe7396764074909f4a1fda80719bc5b79fa5 +Subproject commit a4407d6049c815f74e7225433353f2d22129cd03 diff --git a/panda b/panda index 0696730c14..01df29e00b 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 0696730c140dfb537e3a102ee6334c334f9a087f +Subproject commit 01df29e00b6d3dcceb7b4da18ddf97118382c659 diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 576831c613..f165eb4332 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -2,10 +2,9 @@ from cereal import car from common.numpy_fast import clip, interp from selfdrive.car.nissan import nissancan from opendbc.can.packer import CANPacker +from selfdrive.car.nissan.values import CAR # 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 @@ -13,8 +12,10 @@ LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower VisualAlert = car.CarControl.HUDControl.VisualAlert + class CarController(): def __init__(self, dbc_name, CP, VM): + self.CP = CP self.car_fingerprint = CP.carFingerprint self.lkas_max_torque = 0 @@ -31,7 +32,6 @@ class CarController(): ### STEER ### acc_active = bool(CS.out.cruiseState.enabled) - cruise_throttle_msg = CS.cruise_throttle_msg lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg apply_angle = actuators.steerAngle @@ -45,12 +45,7 @@ class CarController(): 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) + apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): @@ -70,9 +65,15 @@ class CarController(): # 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)) + if self.CP.carFingerprint == CAR.XTRAIL and cruise_cancel: + can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, CS.cruise_throttle_msg, frame)) + + # TODO: Find better way to cancel! + # For some reason spamming the cancel button is unreliable on the Leaf + # We now cancel by making propilot think the seatbelt is unlatched, + # this generates a beep and a warning message every time you disengage + if self.CP.carFingerprint == CAR.LEAF and frame % 2 == 0: + can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) can_sends.append(nissancan.create_steering_control( self.packer, self.car_fingerprint, apply_angle, frame, acc_active, self.lkas_max_torque)) diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 9eb6589510..4930e8a1f4 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -4,22 +4,33 @@ 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 +from selfdrive.car.nissan.values import CAR, DBC, STEER_THRESHOLD 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["GAS_PEDAL"]["GAS_PEDAL"] - ret.gasPressed = bool(ret.gas) - ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) - ret.brakeLights = bool(cp.vl["DOORS_LIGHTS"]["BRAKE_LIGHT"]) + if self.CP.carFingerprint == CAR.XTRAIL: + ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] + elif self.CP.carFingerprint == CAR.LEAF: + ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] + + ret.gasPressed = bool(ret.gas > 3) + + if self.CP.carFingerprint == CAR.XTRAIL: + ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) + elif self.CP.carFingerprint == CAR.LEAF: + ret.brakePressed = bool(cp.vl["BRAKE_PEDAL"]["BRAKE_PEDAL"] > 3) + + if self.CP.carFingerprint == CAR.XTRAIL: + ret.brakeLights = bool(cp.vl["DOORS_LIGHTS"]["BRAKE_LIGHT"]) ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS @@ -28,33 +39,50 @@ class CarState(CarStateBase): 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.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + if self.CP.carFingerprint == CAR.XTRAIL: + ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) + elif self.CP.carFingerprint == CAR.LEAF: + ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) + + # TODO: Find mph/kph bit on XTRAIL + if self.CP.carFingerprint == CAR.LEAF: + speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] + if speed != 255: + speed -= 1 # Speed on HUD is always 1 lower than actually sent on can bus + conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS + ret.cruiseState.speed = speed * conversion + + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + + ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] - # Don't see this in the drive ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) - ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_UNLATCHED"] == 0 - ret.cruiseState.enabled = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ACTIVATED"]) - ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) - ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"], cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"], cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FR"], cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FL"]]) - ret.steeringPressed = bool(cp.vl["STEER_TORQUE_SENSOR2"]["STEERING_PRESSED"]) - ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] - ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 ret.espDisabled = bool(cp.vl["ESP"]["ESP_DISABLED"]) + can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + + self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) + + if self.CP.carFingerprint == CAR.LEAF: + self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) + self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) @@ -70,40 +98,22 @@ class CarState(CarStateBase): ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR", 0), ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR", 0), - ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT", 0), + + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), + ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), ("DOOR_OPEN_FR", "DOORS_LIGHTS", 1), ("DOOR_OPEN_FL", "DOORS_LIGHTS", 1), ("DOOR_OPEN_RR", "DOORS_LIGHTS", 1), ("DOOR_OPEN_RL", "DOORS_LIGHTS", 1), - ("USER_BRAKE_PRESSED", "DOORS_LIGHTS", 1), - ("BRAKE_LIGHT", "DOORS_LIGHTS", 1), - - ("GAS_PEDAL", "GAS_PEDAL", 0), - - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ("STEERING_PRESSED", "STEER_TORQUE_SENSOR2", 0), - - ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), - ("RIGHT_BLINKER", "LIGHTS", 0), ("LEFT_BLINKER", "LIGHTS", 0), - ("PROPILOT_BUTTON", "CRUISE_THROTTLE", 0), - ("CANCEL_BUTTON", "CRUISE_THROTTLE", 0), - ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE", 0), - ("SET_BUTTON", "CRUISE_THROTTLE", 0), - ("RES_BUTTON", "CRUISE_THROTTLE", 0), - ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE", 0), - ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE", 0), - ("GAS_PEDAL", "CRUISE_THROTTLE", 0), - ("unsure1", "CRUISE_THROTTLE", 0), - ("unsure2", "CRUISE_THROTTLE", 0), - ("unsure3", "CRUISE_THROTTLE", 0), - # TODO: why are USER_BRAKE_PRESSED, NEW_SIGNAL_2 and GAS_PRESSED_INVERTED not forwarded + ("SEATBELT_DRIVER_LATCHED", "SEATBELT", 0), ("ESP_DISABLED", "ESP", 0), + ("GEAR_SHIFTER", "GEARBOX", 0), ] @@ -111,9 +121,57 @@ class CarState(CarStateBase): # sig_address, frequency ("WHEEL_SPEEDS_REAR", 50), ("WHEEL_SPEEDS_FRONT", 50), + ("STEER_TORQUE_SENSOR", 100), + ("STEER_ANGLE_SENSOR", 100), ("DOORS_LIGHTS", 10), ] + if CP.carFingerprint == CAR.XTRAIL: + signals += [ + ("USER_BRAKE_PRESSED", "DOORS_LIGHTS", 1), + ("BRAKE_LIGHT", "DOORS_LIGHTS", 1), + + ("GAS_PEDAL", "GAS_PEDAL", 0), + + ("PROPILOT_BUTTON", "CRUISE_THROTTLE", 0), + ("CANCEL_BUTTON", "CRUISE_THROTTLE", 0), + ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE", 0), + ("SET_BUTTON", "CRUISE_THROTTLE", 0), + ("RES_BUTTON", "CRUISE_THROTTLE", 0), + ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE", 0), + ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE", 0), + ("GAS_PEDAL", "CRUISE_THROTTLE", 0), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 0), + ("NEW_SIGNAL_2", "CRUISE_THROTTLE", 0), + ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE", 0), + ("unsure1", "CRUISE_THROTTLE", 0), + ("unsure2", "CRUISE_THROTTLE", 0), + ("unsure3", "CRUISE_THROTTLE", 0), + ] + + checks += [ + ("GAS_PEDAL", 50), + ] + + elif CP.carFingerprint == CAR.LEAF: + signals += [ + ("BRAKE_PEDAL", "BRAKE_PEDAL", 0), + + ("GAS_PEDAL", "CRUISE_THROTTLE", 0), + ("CRUISE_AVAILABLE", "CRUISE_THROTTLE", 0), + ("SPEED_MPH", "HUD_SETTINGS", 0), + + # Copy other values, we use this to cancel + ("CANCEL_SEATBELT", "CANCEL_MSG", 0), + ("NEW_SIGNAL_1", "CANCEL_MSG", 0), + ("NEW_SIGNAL_2", "CANCEL_MSG", 0), + ("NEW_SIGNAL_3", "CANCEL_MSG", 0), + ] + checks += [ + ("BRAKE_PEDAL", 100), + ("CRUISE_THROTTLE", 50), + ] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @staticmethod @@ -121,6 +179,10 @@ class CarState(CarStateBase): # this function generates lists for signal, messages and initial values signals = [ # sig_name, sig_address, default + ("LKAS_ENABLED", "LKAS_SETTINGS", 0), + + ("CRUISE_ENABLED", "CRUISE_STATE", 0), + ("DESIRED_ANGLE", "LKAS", 0), ("SET_0x80_2", "LKAS", 0), ("MAX_TORQUE", "LKAS", 0), @@ -150,7 +212,7 @@ class CarState(CarStateBase): ("unknown26", "PROPILOT_HUD", 0), ("unknown28", "PROPILOT_HUD", 0), ("unknown31", "PROPILOT_HUD", 0), - ("unknown39", "PROPILOT_HUD", 0), + ("SET_SPEED", "PROPILOT_HUD", 0), ("unknown43", "PROPILOT_HUD", 0), ("unknown08", "PROPILOT_HUD", 0), ("unknown05", "PROPILOT_HUD", 0), @@ -195,22 +257,21 @@ class CarState(CarStateBase): ("unknown61", "PROPILOT_HUD_INFO_MSG", 0), ("unknown55", "PROPILOT_HUD_INFO_MSG", 0), ("unknown50", "PROPILOT_HUD_INFO_MSG", 0), - ] checks = [ - # sig_address, frequency + ("CRUISE_STATE", 50), ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) @staticmethod def get_cam_can_parser(CP): - signals = [ - ("CRUISE_ON", "PRO_PILOT", 0), - ("CRUISE_ACTIVATED", "PRO_PILOT", 0), - ("STEER_STATUS", "PRO_PILOT", 0), - ] + signals = [] + if CP.carFingerprint == CAR.XTRAIL: + signals += [ + ("CRUISE_ON", "PRO_PILOT", 0), + ] checks = [ ] diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 7aee3dea9b..676ef66a3f 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -19,7 +19,6 @@ class CarInterface(CarInterfaceBase): 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.safetyModel = car.CarParams.SafetyModel.nissan @@ -27,17 +26,23 @@ class CarInterface(CarInterfaceBase): ret.enableCamera = True ret.steerRateCost = 0.5 - if candidate in [CAR.XTRAIL]: + 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.] + + if candidate == CAR.XTRAIL: + ret.mass = 1610 + STD_CARGO_KG + ret.wheelbase = 2.705 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 17 + elif candidate == CAR.LEAF: 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.radarOffCan = True @@ -75,6 +80,9 @@ class CarInterface(CarInterfaceBase): if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + if self.CS.lkas_enabled: + events.append(create_event('invalidLkasSetting', [ET.PERMANENT])) + ret.events = events # update previous brake/gas pressed @@ -82,6 +90,7 @@ class CarInterface(CarInterfaceBase): self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled + self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index 3fb8d53dbd..6786774c3e 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -6,21 +6,19 @@ 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["CHECKSUM"] = nissan_checksum(dat[:7]) - + 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["CHECKSUM"] = nissan_checksum(dat[:7]) return packer.make_can_msg("LKAS", 0, values) @@ -38,6 +36,15 @@ def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame): return packer.make_can_msg("CRUISE_THROTTLE", 2, values) +def create_cancel_msg(packer, cancel_msg, cruise_cancel): + values = copy.copy(cancel_msg) + + if cruise_cancel: + values["CANCEL_SEATBELT"] = 1 + + return packer.make_can_msg("CANCEL_MSG", 2, values) + + def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart): values = lkas_hud_msg diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 857abff4e3..96777e5a44 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -1,8 +1,10 @@ from selfdrive.car import dbc_dict +STEER_THRESHOLD = 1.0 class CAR: XTRAIL = "NISSAN X-TRAIL 2017" + LEAF = "NISSAN LEAF 2018" FINGERPRINTS = { @@ -13,9 +15,15 @@ FINGERPRINTS = { { 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 }, - ] + ], + CAR.LEAF: [ + { + 2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8 + }, + ], } DBC = { CAR.XTRAIL: dbc_dict('nissan_x_trail_2017', None), + CAR.LEAF: dbc_dict('nissan_leaf_2018', None), } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 121b4aba16..7d272fb8ce 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -27,6 +27,8 @@ from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter LANE_DEPARTURE_THRESHOLD = 0.1 +SATURATION_TIMEOUT_ON_ENGAGE = 2.0 / DT_CTRL +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState @@ -218,7 +220,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): + AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, active_count): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -226,6 +228,8 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr enabled = isEnabled(state) active = isActive(state) + active_count = active_count + 1 if active else 0 + if CS.leftBlinker or CS.rightBlinker: last_blinker_frame = frame @@ -266,8 +270,13 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) + # Check for difference between desired angle and angle for angle based control + angle_control_saturated = CP.steerControlType == car.CarParams.SteerControlType.angle and \ + abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD + # Send a "steering required alert" if saturation count has reached the limit - if lac_log.saturated and not CS.steeringPressed: + # TODO: add a minimum duration, now a warning will show every time the path changes briefly + if (active_count > SATURATION_TIMEOUT_ON_ENGAGE) and (lac_log.saturated or angle_control_saturated) and not CS.steeringPressed: # Check if we deviated from the path left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 @@ -286,7 +295,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) - return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame + return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame, active_count def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, @@ -455,7 +464,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \ 'model']) - if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock('can', timeout=can_timeout) @@ -505,6 +513,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): mismatch_counter = 0 can_error_counter = 0 last_blinker_frame = 0 + active_count = 0 events_prev = [] sm['liveCalibration'].calStatus = Calibration.INVALID @@ -571,9 +580,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame = \ + actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, active_count = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) + LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, active_count) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 88fb8fbb03..d548480a5a 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -723,6 +723,13 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( + "invalidLkasSettingPermanent", + "Stock LKAS is turned on", + "Turn off stock LKAS to engage", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( "internetConnectivityNeededPermanent", "Please connect to Internet", diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index fd7a8e60be..2aebe4cef8 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -57,6 +57,7 @@ def get_route_log(route_name): print("failed to download test log %s" % route_name) sys.exit(-1) + routes = { "420a8e183f1aed48|2020-03-05--07-15-29": { 'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID, @@ -335,6 +336,10 @@ routes = { 'carFingerprint': NISSAN.XTRAIL, 'enableCamera': True, }, + "5b7c365c50084530|2020-03-25--22-10-13": { + 'carFingerprint': NISSAN.LEAF, + 'enableCamera': True, + }, } passive_routes = [ @@ -344,9 +349,6 @@ 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: add routes for these cars