sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Process gear and brake signals, add new fingerprints

Browse Source 
Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>
pull/988/head
Jafar Al-Gharaibeh 5 years ago committed by Adeeb Shihadeh
parent 51582596c9
commit bef76d69f1
2 changed files with 102 additions and 62 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 136
      selfdrive/car/mazda/carstate.py
  2. 28
      selfdrive/car/mazda/values.py

136
selfdrive/car/mazda/carstate.py
Unescape View File

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

28
selfdrive/car/mazda/values.py
Unescape View File

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

Write Preview
Loading…
Cancel
Save