Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>
pull/988/head
Jafar Al-Gharaibeh 6 years ago committed by Adeeb Shihadeh
parent 7961d5ffd7
commit 44c36ab275
  1. 1
      selfdrive/car/mazda/__init__.py
  2. 35
      selfdrive/car/mazda/carcontroller.py
  3. 197
      selfdrive/car/mazda/carstate.py
  4. 147
      selfdrive/car/mazda/interface.py
  5. 37
      selfdrive/car/mazda/mazdacan.py
  6. 6
      selfdrive/car/mazda/radar_interface.py
  7. 45
      selfdrive/car/mazda/values.py
  8. 7
      selfdrive/test/test_car_models.py

@ -0,0 +1,35 @@
from selfdrive.car.mazda import mazdacan
from selfdrive.car.mazda.values import DBC, SteerLimitParams
from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_torque_limits
class CarController():
def __init__(self, dbc_name, CP, VM):
self.steer_idx = 0
self.apply_steer_last = 0
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
def update(self, enabled, CS, frame, actuators):
""" Controls thread """
can_sends = []
### STEER ###
if enabled and not CS.steer_not_allowed:
# calculate steer and also set limits due to driver torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steer_torque_driver, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
self.steer_rate_limited = False
self.apply_steer_last = apply_steer
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends

@ -0,0 +1,197 @@
from cereal import car
from selfdrive.config import Conversions as CV
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.mazda.values import DBC, LKAS_LIMITS
GearShifter = car.CarState.GearShifter
class STEER_LKAS():
def __init__(self):
self.block = 1
self.track = 1
self.handsoff = 0
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.steer_lkas = STEER_LKAS()
self.acc_active_last = False
self.speed_kph = 0
self.lkas_speed_lock = False
self.low_speed_lockout = True
self.low_speed_lockout_last = True
self.acc_press_update = False
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
ret.gearShifter = GearShifter.drive
self.speed_kph = ret.vEgoRaw // CV.KPH_TO_MS
ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1
ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1
ret.steeringAngle = cp.vl["STEER"]['STEER_ANGLE']
ret.steeringTorque = cp.vl["STEER_TORQUE"]['STEER_TORQUE_SENSOR']
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
self.steer_torque_motor = cp.vl["STEER_TORQUE"]['STEER_TORQUE_MOTOR']
self.angle_steers_rate = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE']
# TODO: Find brake & brake pressure
ret.brake = 0
self.brake_pressed = False #cp.vl["PEDALS"]['BREAK_PEDAL_1'] == 1
ret.seatbeltUnlatched = cp.vl["SEATBELT"]['DRIVER_SEATBELT'] == 0
ret.doorOpen = any([cp.vl["DOORS"]['FL'],
cp.vl["DOORS"]['FR'],
cp.vl["DOORS"]['BL'],
cp.vl["DOORS"]['BR']])
ret.gasPressed = cp.vl["ENGINE_DATA"]['PEDAL_GAS'] > 0.0001
#TODO get gear state
#ret.gearShifter
# No steer if block signal is on
self.steer_lkas.block = cp.vl["STEER_RATE"]['LKAS_BLOCK']
# track driver torque, on if torque is not detected
self.steer_lkas.track = cp.vl["STEER_RATE"]['LKAS_TRACK_STATE']
# On if no driver torque the last 5 seconds
self.steer_lkas.handsoff = cp.vl["STEER_RATE"]['HANDS_OFF_5_SECONDS']
# LKAS is enabled at 50kph going up and disabled at 45kph going down
if self.speed_kph > LKAS_LIMITS.ENABLE_SPEED and self.low_speed_lockout:
self.low_speed_lockout = False
elif self.speed_kph < LKAS_LIMITS.DISABLE_SPEED and not self.low_speed_lockout:
self.low_speed_lockout = True
if (self.low_speed_lockout or self.steer_lkas.block) and self.speed_kph < LKAS_LIMITS.DISABLE_SPEED:
if not self.lkas_speed_lock:
self.lkas_speed_lock = True
elif self.lkas_speed_lock:
self.lkas_speed_lock = False
# if any of the cruize buttons is pressed force state update
if any([cp.vl["CRZ_BTNS"]['RES'],
cp.vl["CRZ_BTNS"]['SET_P'],
cp.vl["CRZ_BTNS"]['SET_M']]):
self.acc_active = True
ret.cruiseState.speed = self.speed_kph
if self.low_speed_lockout_last:
self.acc_press_update = True
elif self.acc_press_update:
self.acc_press_update = False
ret.cruiseState.available = cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1
if not ret.cruiseState.available:
self.acc_active = False
if self.acc_active != self.acc_active_last:
ret.cruiseState.speed = self.speed_kph
self.acc_active_last = self.acc_active
ret.cruiseState.enabled = self.acc_active
self.steer_error = False
self.brake_error = False
self.low_speed_lockout_last = self.low_speed_lockout
#self.steer_not_allowed = self.steer_lkas.block == 1
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
return ret
@staticmethod
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("LEFT_BLINK", "BLINK_INFO", 0),
("RIGHT_BLINK", "BLINK_INFO", 0),
("STEER_ANGLE", "STEER", 0),
("STEER_ANGLE_RATE", "STEER_RATE", 0),
("LKAS_BLOCK", "STEER_RATE", 0),
("LKAS_TRACK_STATE", "STEER_RATE", 0),
("HANDS_OFF_5_SECONDS", "STEER_RATE", 0),
("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0),
("STEER_TORQUE_MOTOR", "STEER_TORQUE", 0),
("FL", "WHEEL_SPEEDS", 0),
("FR", "WHEEL_SPEEDS", 0),
("RL", "WHEEL_SPEEDS", 0),
("RR", "WHEEL_SPEEDS", 0),
("CRZ_ACTIVE", "CRZ_CTRL", 0),
("STANDSTILL","PEDALS", 0),
("BRAKE_ON","PEDALS", 0),
("GEAR","GEAR", 0),
("DRIVER_SEATBELT", "SEATBELT", 0),
("FL", "DOORS", 0),
("FR", "DOORS", 0),
("BL", "DOORS", 0),
("BR", "DOORS", 0),
("PEDAL_GAS", "ENGINE_DATA", 0),
("RES", "CRZ_BTNS", 0),
("SET_P", "CRZ_BTNS", 0),
("SET_M", "CRZ_BTNS", 0),
]
checks = [
# sig_address, frequency
("BLINK_INFO", 10),
("STEER", 67),
("STEER_RATE", 83),
("STEER_TORQUE", 83),
("WHEEL_SPEEDS", 100),
("ENGINE_DATA", 100),
("CRZ_CTRL", 50),
("CRZ_BTNS", 10),
("PEDALS", 50),
("SEATBELT", 10),
("DOORS", 10),
("GEAR", 20),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("LKAS_REQUEST", "CAM_LKAS", 0),
("CTR", "CAM_LKAS", 0),
("ERR_BIT_1", "CAM_LKAS", 0),
("LDW", "CAM_LKAS", 0),
("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
("BIT_1", "CAM_LKAS", 0),
("ERR_BIT_2", "CAM_LKAS", 0),
("BIT_2", "CAM_LKAS", 0),
("CHKSUM", "CAM_LKAS", 0),
]
checks = [
# sig_address, frequency
("CAM_LKAS", 16),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)

@ -0,0 +1,147 @@
#!/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.mazda.values import CAR, FINGERPRINTS, ECU_FINGERPRINT, ECU
from selfdrive.car.mazda.carstate import CarState
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected
from selfdrive.car.interfaces import CarInterfaceBase
class CanBus():
def __init__(self):
self.powertrain = 0
self.obstacle = 1
self.cam = 2
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
self.gas_pressed_prev = False
self.low_speed_alert = False
@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 = car.CarParams.new_message()
ret.carName = "mazda"
ret.radarOffCan = True
ret.carFingerprint = candidate
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.mazda
ret.enableCruise = True
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
tire_stiffness_factor = 0.70 # not optimized yet
if candidate in [CAR.CX5]:
ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.7
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.2]]
ret.lateralTuning.pid.kf = 0.00006
ret.steerLimitTimer = 0.8
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
ret.steerRatioRear = 0.
ret.steerControlType = car.CarParams.SteerControlType.torque
# steer limitations VS speed
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.]
# No long control in Mazda
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.openpilotLongitudinalControl = False
ret.stoppingControl = False
ret.startAccel = 0.0
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
# no steer below 45kph
ret.minSteerSpeed = 45 * CV.KPH_TO_MS
# 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,
tire_stiffness_factor=tire_stiffness_factor)
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)
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# TODO: button presses
ret.buttonEvents = []
events = self.create_common_events(ret)
if ret.cruiseState.enabled and self.CS.lkas_speed_lock:
self.low_speed_alert = True
else:
self.low_speed_alert = False
# events
events = self.create_common_events(ret)
if self.CS.low_speed_lockout:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
ret.cruiseState.enabled = False
if self.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
if self.CS.steer_lkas.handsoff:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if (ret.gasPressed and not self.gas_pressed_prev):
ret.cruiseState.enabled = False
ret.events = events
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)
self.frame += 1
return can_sends

@ -0,0 +1,37 @@
from selfdrive.car.mazda.values import CAR
def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
tmp = apply_steer + 2048
lo = tmp & 0xFF
hi = tmp >> 8
b1 = int(lkas["BIT_1"])
b2 = int(lkas[ "BIT_2"])
ldw = int(lkas["LDW"])
lnv = 0 #int(lkas["LINE_NOT_VISIBLE"])
ctr = frame % 16
csum = 241 - ctr - (hi - 8) - lo - (lnv << 3) - (b1 << 5) - (b2 << 1) - (ldw << 7)
if csum < 0:
csum = csum + 256
csum = csum % 256
if car_fingerprint == CAR.CX5:
values = {
"CTR" : ctr,
"LKAS_REQUEST" : apply_steer,
"BIT_1" : b1,
"BIT_2" : b2,
"LDW" : ldw,
"LINE_NOT_VISIBLE" : lnv,
"ERR_BIT_1" : 0,
"ERR_BIT_2" : 0,
"CHKSUM" : csum
}
return packer.make_can_msg("CAM_LKAS", 0, values)

@ -0,0 +1,6 @@
#!/usr/bin/env python3
from selfdrive.car.interfaces import RadarInterfaceBase
class RadarInterface(RadarInterfaceBase):
pass

@ -0,0 +1,45 @@
from selfdrive.car import dbc_dict
# Steer torque limits
class SteerLimitParams:
STEER_MAX = 600 # max_steer 2048
STEER_STEP = 1 # how often we update the steer cmd
STEER_DELTA_UP = 10 # torque increase per refresh
STEER_DELTA_DOWN = 20 # torque decrease per refresh
STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting
STEER_DRIVER_MULTIPLIER = 1 # weight driver torque heavily
STEER_DRIVER_FACTOR = 1 # from dbc
class CAR:
CX5 = "Mazda CX-5 GT 2017"
class LKAS_LIMITS:
STEER_THRESHOLD = 20
DISABLE_SPEED = 45
ENABLE_SPEED = 50
FINGERPRINTS = {
CAR.CX5: [
# CX-5 2017 GT
{
64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 608: 8, 628: 8, 832: 8, 836: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1070: 8, 1078: 8, 1080: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1139: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1275: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1446: 8, 1456: 8, 1479: 8
},
# CX-5 2019 GTR
{
64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 254: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 608: 8, 628: 8, 736: 8, 832: 8, 836: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1078: 8, 1080: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1139: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1170: 8, 1171: 8, 1173: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1244: 8, 1260: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1446: 8, 1456: 8, 1479: 8, 1776: 8, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8
}
],
}
class ECU:
CAM = 0
ECU_FINGERPRINT = {
ECU.CAM: [579], # steer torque cmd
}
DBC = {
CAR.CX5: dbc_dict('mazda_cx5_gt_2017', None),
}

@ -20,6 +20,7 @@ from selfdrive.car.gm.values import CAR as GM
from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.hyundai.values import CAR as HYUNDAI
from selfdrive.car.nissan.values import CAR as NISSAN
from selfdrive.car.mazda.values import CAR as MAZDA
from selfdrive.car.subaru.values import CAR as SUBARU
from selfdrive.car.toyota.values import CAR as TOYOTA
from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN
@ -362,6 +363,10 @@ routes = {
'carFingerprint': NISSAN.LEAF,
'enableCamera': True,
},
"32a319f057902bb3|2020-04-22--21-27-53": {
'carFingerprint': MAZDA.CX5,
'enableCamera': True,
},
}
passive_routes: List[str] = [
@ -371,6 +376,8 @@ forced_dashcam_routes = [
# Ford fusion
"f1b4c567731f4a1b|2018-04-18--11-29-37",
"f1b4c567731f4a1b|2018-04-30--10-15-35",
# Mazda
#"32a319f057902bb3|2020-04-22--21-27-53",
]
# TODO: add routes for these cars

Loading…
Cancel
Save