diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index afd986b82e..7fc2f0a557 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -1,15 +1,17 @@ from cereal import car from selfdrive.config import Conversions as CV +from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, CAR -GearShifter = car.CarState.GearShifter - class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.shifter_values = can_define.dv["GEAR"]['GEAR'] + self.cruise_speed = 0 self.acc_active_last = False self.speed_kph = 0 @@ -31,18 +33,8 @@ class CarState(CarStateBase): 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 + can_gear = int(cp.vl["GEAR"]['GEAR']) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1 ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1 @@ -89,7 +81,7 @@ class CarState(CarStateBase): cp.vl["CRZ_BTNS"]['SET_P'], cp.vl["CRZ_BTNS"]['SET_M']]): self.acc_active = True - self.cruise_speed = self.speed_kph + self.cruise_speed = ret.vEgoRaw if self.low_speed_lockout_last: self.acc_press_update = True elif self.acc_press_update: @@ -195,8 +187,8 @@ class CarState(CarStateBase): ("LDW", "CAM_LKAS", 0), ("BIT_1", "CAM_LKAS", 1), ("ERR_BIT_2", "CAM_LKAS", 0), - ("LKAS_ANGLE", "CAM_LKAS", 2048), - ("BIT2", "CAM_LKAS", 0), + ("STEERING_ANGLE", "CAM_LKAS", 2048), + ("ANGLE_ENABLED", "CAM_LKAS", 0), ("CHKSUM", "CAM_LKAS", 0), ] diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 540c77934f..931673423a 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -10,13 +10,13 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): b1 = int(lkas["BIT_1"]) ldw = int(lkas["LDW"]) er1= int(lkas["ERR_BIT_1"]) - lnv = 0 #int(lkas["LINE_NOT_VISIBLE"]) + lnv = 0 er2= int(lkas["ERR_BIT_2"]) - lkas_angle = lkas["LKAS_ANGLE"] - b2 = int(lkas["BIT2"]) + steering_angle = int(lkas["STEERING_ANGLE"]) + b2 = int(lkas["ANGLE_ENABLED"]) - tmp = int((lkas_angle + 45.06 ) / 0.022) + tmp = steering_angle + 2048 ahi = tmp >> 10 amd = (tmp & 0x3FF) >> 2 amd = (amd >> 4) | (( amd & 0xF) << 4) @@ -49,8 +49,8 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): "LDW" : ldw, "BIT_1" : b1, "ERR_BIT_2" : er2, - "LKAS_ANGLE" : lkas_angle, - "BIT2" : b2, + "STEERING_ANGLE" : steering_angle, + "ANGLE_ENABLED" : b2, "CHKSUM" : csum }