Nissan leaf (#1275)

* split out leaf and xtrail

* Add brake pedal

* This should work

* Fix test car models + bump panda

* Combined cruise enabled detection in single message

* Proper frequency checks

* Add doors

* Blinkers and doors

* Seatbelt

* Gear

* Add cancel message

* Unify steering pressed

* Remove angle limit

* Add steer saturation alert for angle based control

* Add set speed

* Change wheel speed factor

* Fix offset in set speed

* Timeout on engage for steer saturated

* Put counter back

* try cancel using seatbelt

* Try different cancel message

* Rename cancel signal

* Add LKAS alert

* Add missing fingerprint values

* Update test car models

* Add some comments

cereal

@@ -1 +1 @@
-Subproject commit b0c746b1e1e3ca04f61484fd073123a1bf96afd1
+Subproject commit f515e4db2e9941158d1c477f35b186df5633aa2e

opendbc

@@ -1 +1 @@
-Subproject commit 50fbbe7396764074909f4a1fda80719bc5b79fa5
+Subproject commit a4407d6049c815f74e7225433353f2d22129cd03

panda

@@ -1 +1 @@
-Subproject commit 0696730c140dfb537e3a102ee6334c334f9a087f
+Subproject commit 01df29e00b6d3dcceb7b4da18ddf97118382c659

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 -
+      apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
-                         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):
@@ -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:
+    if self.CP.carFingerprint == CAR.XTRAIL and cruise_cancel:
-      can_sends.append(nissancan.create_acc_cancel_cmd(
+        can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, CS.cruise_throttle_msg, frame))
-          self.packer, 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))

selfdrive/car/nissan/carstate.py

@@ -4,21 +4,32 @@ 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()
 
+    if self.CP.carFingerprint == CAR.XTRAIL:
       ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
-    ret.gasPressed = bool(ret.gas)
+    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
@@ -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.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"])
-    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
+    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.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0
-    ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
-    ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
 
     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,25 +98,40 @@ 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),
+      ("RIGHT_BLINKER", "LIGHTS", 0),
-      ("BRAKE_LIGHT", "DOORS_LIGHTS", 1),
+      ("LEFT_BLINKER", "LIGHTS", 0),
 
-      ("GAS_PEDAL", "GAS_PEDAL", 0),
+      ("SEATBELT_DRIVER_LATCHED", "SEATBELT", 0),
 
-      ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
+      ("ESP_DISABLED", "ESP", 0),
-      ("STEERING_PRESSED", "STEER_TORQUE_SENSOR2", 0),
 
-      ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
+      ("GEAR_SHIFTER", "GEARBOX", 0),
+    ]
 
-      ("RIGHT_BLINKER", "LIGHTS", 0),
+    checks = [
-      ("LEFT_BLINKER", "LIGHTS", 0),
+      # 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),
@@ -98,20 +141,35 @@ class CarState(CarStateBase):
         ("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),
-      # TODO: why are USER_BRAKE_PRESSED, NEW_SIGNAL_2 and GAS_PRESSED_INVERTED  not forwarded
+      ]
 
-      ("ESP_DISABLED", "ESP", 0),
+      checks += [
-      ("GEAR_SHIFTER", "GEARBOX", 0),
+        ("GAS_PEDAL", 50),
       ]
 
-    checks = [
+    elif CP.carFingerprint == CAR.LEAF:
-      # sig_address, frequency
+      signals += [
-      ("WHEEL_SPEEDS_REAR", 50),
+        ("BRAKE_PEDAL", "BRAKE_PEDAL", 0),
-      ("WHEEL_SPEEDS_FRONT", 50),
+
-      ("DOORS_LIGHTS", 10),
+        ("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)
@@ -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,21 +257,20 @@ 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 = [
+    signals = []
+    if CP.carFingerprint == CAR.XTRAIL:
+      signals += [
         ("CRUISE_ON", "PRO_PILOT", 0),
-      ("CRUISE_ACTIVATED", "PRO_PILOT", 0),
-      ("STEER_STATUS", "PRO_PILOT", 0),
       ]
 
     checks = [

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,11 +26,6 @@ class CarInterface(CarInterfaceBase):
     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]]
@@ -39,6 +33,17 @@ class CarInterface(CarInterfaceBase):
     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.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
 

selfdrive/car/nissan/nissancan.py

@@ -6,7 +6,6 @@ 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,
@@ -20,7 +19,6 @@ def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_o
   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
 

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),
 }

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")
 

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",

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