Mazda Port

Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>

selfdrive/car/mazda/__init__.py

@@ -0,0 +1 @@
+

selfdrive/car/mazda/carcontroller.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

selfdrive/car/mazda/carstate.py

@@ -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)
+

selfdrive/car/mazda/interface.py

@@ -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

selfdrive/car/mazda/mazdacan.py

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

selfdrive/car/mazda/radar_interface.py

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

selfdrive/car/mazda/values.py

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

selfdrive/test/test_car_models.py

@@ -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