diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index cd98ce96d1..bfa1a56f60 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -2,8 +2,7 @@ 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 - +from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, CAR GearShifter = car.CarState.GearShifter @@ -33,16 +32,25 @@ class CarState(CarStateBase): 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 + gear = int(cp.vl["GEAR"]['GEAR']) >> 1 + + if gear >= 1 & gear <= 5: + ret.gearShifter = GearShifter.drive + elif gear == 0: + ret.gearShifter = GearShifter.park + elif gear == 6: + ret.gearShifter = GearShifter.reverse + elif gear == 7: + ret.gearShifter = GearShifter.neutral + else: + ret.gearShifter = GearShifter.unknown + ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1 ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1 @@ -53,20 +61,16 @@ class CarState(CarStateBase): 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.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1 + ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE'] + ret.brakeLights = ret.brakePressed 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 + ret.doorOpen = any([cp.vl["DOORS"]['FL'], cp.vl["DOORS"]['FR'], + cp.vl["DOORS"]['BL'], cp.vl["DOORS"]['BR']]) - #TODO get gear state - #ret.gearShifter + ret.gas = cp.vl["ENGINE_DATA"]['PEDAL_GAS'] + ret.gasPressed = ret.gas > 1e-3 # No steer if block signal is on self.steer_lkas.block = cp.vl["STEER_RATE"]['LKAS_BLOCK'] @@ -129,70 +133,82 @@ class CarState(CarStateBase): ("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), + ("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), ] + if CP.carFingerprint == CAR.CX5: + signals += [ + ("LKAS_BLOCK", "STEER_RATE", 0), + ("LKAS_TRACK_STATE", "STEER_RATE", 0), + ("HANDS_OFF_5_SECONDS", "STEER_RATE", 0), + ("CRZ_ACTIVE", "CRZ_CTRL", 0), + ("STANDSTILL","PEDALS", 0), + ("BRAKE_ON","PEDALS", 0), + ("BRAKE_PRESSURE","BRAKE", 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 += [ + ("ENGINE_DATA", 100), + ("CRZ_CTRL", 50), + ("CRZ_BTNS", 10), + ("PEDALS", 50), + ("BRAKE", 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", 2048), - ("CTR", "CAM_LKAS", 0), - ("ERR_BIT_1", "CAM_LKAS", 0), - ("LINE_NOT_VISIBLE", "CAM_LKAS", 0), - ("LDW", "CAM_LKAS", 0), - ("BIT_1", "CAM_LKAS", 1), - ("ERR_BIT_2", "CAM_LKAS", 0), - ("LKAS_ANGLE", "CAM_LKAS", 2048), - ("BIT2", "CAM_LKAS", 0), - ("CHKSUM", "CAM_LKAS", 0), - ] - - checks = [ - # sig_address, frequency - ("CAM_LKAS", 16), - ] + signals = [ ] + checks = [ ] + + if CP.carFingerprint == CAR.CX5: + signals += [ + # sig_name, sig_address, default + ("LKAS_REQUEST", "CAM_LKAS", 2048), + ("CTR", "CAM_LKAS", 0), + ("ERR_BIT_1", "CAM_LKAS", 0), + ("LINE_NOT_VISIBLE", "CAM_LKAS", 0), + ("LDW", "CAM_LKAS", 0), + ("BIT_1", "CAM_LKAS", 1), + ("ERR_BIT_2", "CAM_LKAS", 0), + ("LKAS_ANGLE", "CAM_LKAS", 2048), + ("BIT2", "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/values.py b/selfdrive/car/mazda/values.py index 46ae927c81..63af2ec112 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -12,7 +12,11 @@ class SteerLimitParams: STEER_DRIVER_FACTOR = 1 # from dbc class CAR: - CX5 = "Mazda CX-5 GT 2017" +# CX3 = "Mazda CX-3" + CX5 = "Mazda CX-5" +# CX9 = "Mazda CX-9" +# Mazda6 = "Mazda 6" +# MAZDA3_2019 = "Mazda 3 2019" class LKAS_LIMITS: STEER_THRESHOLD = 20 @@ -28,8 +32,27 @@ FINGERPRINTS = { # 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 - } + }, + + # TODO: add new car model: + # Mazda 6 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, 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, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1182: 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, 1456: 8, 1479: 8 + }, + # TODO: add new car model: + # CX-9 2017 Australia + { + 64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 138: 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, 522: 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, 583: 8, 605: 8, 606: 8, 628: 8, 832: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1078: 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, 1177: 8, 1180: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1247: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 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 + }, ], + +# CAR.MAZDA3_2019: [ +# The new generation Mazda 3 2019 +# { +# 16: 8, 17: 8, 18: 8, 20: 8, 21: 8, 22: 8, 24: 8, 26: 8, 28: 8, 29: 8, 31: 8, 32: 8, 33: 8, 35: 8, 37: 8, 41: 8, 43: 8, 44: 8, 49: 8, 50: 8, 51: 8, 54: 8, 55: 8, 56: 8, 59: 8, 72: 8, 80: 8, 96: 8, 128: 8, 129: 8, 145: 8, 176: 8, 256: 8, 257: 8, 258: 8, 259: 8, 260: 8, 261: 8, 262: 8, 263: 8, 264: 8, 265: 8, 266: 8, 267: 8, 272: 8, 273: 8, 274: 8, 275: 8, 276: 8, 277: 8, 278: 8, 279: 8, 280: 8, 281: 8, 282: 8, 283: 8, 288: 8, 289: 8, 290: 8, 291: 8, 292: 8, 293: 8, 294: 8, 295: 8, 296: 8, 297: 8, 298: 8, 299: 8, 304: 8, 305: 8, 306: 8, 307: 8, 308: 8, 310: 8, 320: 8, 321: 8, 322: 8, 323: 8, 324: 8, 325: 8, 328: 8, 329: 8, 336: 8, 337: 8, 338: 8, 339: 8, 340: 8, 341: 8, 342: 8, 343: 8, 344: 8, 345: 8, 346: 8, 347: 8, 352: 8, 354: 8, 355: 8, 356: 8, 357: 8, 358: 8, 1034: 8, 1035: 8, 1120: 8, 1184: 8, 1192: 8, 1193: 8, 1200: 8, 1209: 8, 1216: 8, 1436: 8, 1475: 8, 2015: 8 +# }, +# ], + } class ECU: @@ -42,4 +65,5 @@ ECU_FINGERPRINT = { DBC = { CAR.CX5: dbc_dict('mazda_cx5_gt_2017', None), +# CAR.MAZDA3_2019: dbc_dict('mazda_3_2019', None), }