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 commentsalbatross
Willem Melching
GitHub
parent
415780213f
commit
1df6b67511
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit b0c746b1e1e3ca04f61484fd073123a1bf96afd1
|
||||
Subproject commit f515e4db2e9941158d1c477f35b186df5633aa2e
|
||||
2
opendbc
2
opendbc
|
|
@ -1 +1 @@
|
|||
Subproject commit 50fbbe7396764074909f4a1fda80719bc5b79fa5
|
||||
Subproject commit a4407d6049c815f74e7225433353f2d22129cd03
|
||||
2
panda
2
panda
|
|
@ -1 +1 @@
|
|||
Subproject commit 0696730c140dfb537e3a102ee6334c334f9a087f
|
||||
Subproject commit 01df29e00b6d3dcceb7b4da18ddf97118382c659
|
||||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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 = [
|
||||
]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
}
|
||||
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])
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue