Process gear and brake signals, add new fingerprints

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

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.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1
-    ret.brake = 0
+    ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE']
-    self.brake_pressed = False #cp.vl["PEDALS"]['BREAK_PEDAL_1'] == 1
+    ret.brakeLights = ret.brakePressed
 
     ret.seatbeltUnlatched = cp.vl["SEATBELT"]['DRIVER_SEATBELT'] == 0
-    ret.doorOpen = any([cp.vl["DOORS"]['FL'],
+    ret.doorOpen = any([cp.vl["DOORS"]['FL'], cp.vl["DOORS"]['FR'],
-                        cp.vl["DOORS"]['FR'],
+                        cp.vl["DOORS"]['BL'], cp.vl["DOORS"]['BR']])
-                        cp.vl["DOORS"]['BL'],
-                        cp.vl["DOORS"]['BR']])
 
-    ret.gasPressed = cp.vl["ENGINE_DATA"]['PEDAL_GAS'] > 0.0001
+    ret.gas = cp.vl["ENGINE_DATA"]['PEDAL_GAS']
-
+    ret.gasPressed = ret.gas > 1e-3
-    #TODO get gear state
-    #ret.gearShifter
 
     # 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 = [
+    signals = [ ]
-      # sig_name, sig_address, default
+    checks = [ ]
-
+
-      ("LKAS_REQUEST",     "CAM_LKAS", 2048),
+    if CP.carFingerprint == CAR.CX5:
-      ("CTR",              "CAM_LKAS", 0),
+      signals += [
-      ("ERR_BIT_1",        "CAM_LKAS", 0),
+        # sig_name, sig_address, default
-      ("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
+        ("LKAS_REQUEST",     "CAM_LKAS", 2048),
-      ("LDW",              "CAM_LKAS", 0),
+        ("CTR",              "CAM_LKAS", 0),
-      ("BIT_1",            "CAM_LKAS", 1),
+        ("ERR_BIT_1",        "CAM_LKAS", 0),
-      ("ERR_BIT_2",        "CAM_LKAS", 0),
+        ("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
-      ("LKAS_ANGLE",       "CAM_LKAS", 2048),
+        ("LDW",              "CAM_LKAS", 0),
-      ("BIT2",             "CAM_LKAS", 0),
+        ("BIT_1",            "CAM_LKAS", 1),
-      ("CHKSUM",           "CAM_LKAS", 0),
+        ("ERR_BIT_2",        "CAM_LKAS", 0),
-    ]
+        ("LKAS_ANGLE",       "CAM_LKAS", 2048),
-
+        ("BIT2",             "CAM_LKAS", 0),
-    checks = [
+        ("CHKSUM",           "CAM_LKAS", 0),
-      # sig_address, frequency
+      ]
-      ("CAM_LKAS",      16),
+
-    ]
+      checks += [
+        # sig_address, frequency
+        ("CAM_LKAS",      16),
+      ]
 
     return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 

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