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_modelspull/214/head
Willem Melching
6 years ago
committed by
GitHub
parent
ca05a18de9
commit
6db044c86c
10 changed files with 407 additions and 3 deletions
@ -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 |
||||
@ -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) |
||||
@ -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 |
||||
@ -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) |
||||
@ -0,0 +1,5 @@ |
||||
#!/usr/bin/env python3 |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
pass |
||||
@ -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), |
||||
} |
||||
@ -1 +1 @@ |
||||
95638846316e5de7f0314ed2330b01428792c889 |
||||
6829c5c76f3527af06e1c2b685f98a5e1bbef00a |
||||
Loading…
Reference in new issue