diff --git a/selfdrive/car/mazda/__init__.py b/selfdrive/car/mazda/__init__.py new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/selfdrive/car/mazda/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py new file mode 100644 index 0000000000..2f78a45583 --- /dev/null +++ b/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 diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py new file mode 100644 index 0000000000..285442da25 --- /dev/null +++ b/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) + diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py new file mode 100755 index 0000000000..e12c4d3c92 --- /dev/null +++ b/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 diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py new file mode 100644 index 0000000000..51c42c277b --- /dev/null +++ b/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) diff --git a/selfdrive/car/mazda/radar_interface.py b/selfdrive/car/mazda/radar_interface.py new file mode 100755 index 0000000000..55f92f6326 --- /dev/null +++ b/selfdrive/car/mazda/radar_interface.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass + diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py new file mode 100644 index 0000000000..46ae927c81 --- /dev/null +++ b/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), +} diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 685cb485f4..88cc17d09b 100755 --- a/selfdrive/test/test_car_models.py +++ b/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