diff --git a/common/params_keys.h b/common/params_keys.h index 2a274bc3d5..4f9f58b9c9 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -144,6 +144,7 @@ inline static std::unordered_map keys = { {"CarSelected3", {PERSISTENT, STRING}}, {"SupportedCars", {PERSISTENT, STRING}}, {"SupportedCars_gm", {PERSISTENT, STRING}}, + {"SupportedCars_bmw", {PERSISTENT, STRING}}, {"ShowDebugUI", {PERSISTENT, INT, "0"}}, {"ShowTpms", {PERSISTENT, INT, "1"}}, {"ShowDateTime", {PERSISTENT, INT, "1"}}, diff --git a/force_bmw.py b/force_bmw.py new file mode 100755 index 0000000000..69d7bf1459 --- /dev/null +++ b/force_bmw.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +""" +Force openpilot to use BMW car interface. +Run this script on the device to force BMW E90 recognition. +""" + +import sys +import os + +# Add openpilot to path +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from openpilot.common.params import Params + +# Set the car to BMW E90 (or change to BMW_E82 if you have E82) +car_model = "BMW_E90" # Change to "BMW_E82" if needed + +print(f"Setting CarSelected2 parameter to: {car_model}") +Params().put("CarSelected2", car_model) +print("Done! Restart openpilot to apply changes.") +print("\nTo verify, run: python3 -c \"from openpilot.common.params import Params; print(Params().get('CarSelected2'))\"") diff --git a/generate_bmw_list.sh b/generate_bmw_list.sh new file mode 100755 index 0000000000..0a4ac4f8cb --- /dev/null +++ b/generate_bmw_list.sh @@ -0,0 +1,9 @@ +#!/bin/bash +# Generate BMW car list for UI +# Run this script on the device to create the SupportedCars_bmw file + +cd /data/openpilot/system/manager +python3 ../../selfdrive/car/bmw/values.py > /data/params/d/SupportedCars_bmw + +echo "BMW car list generated at /data/params/d/SupportedCars_bmw" +cat /data/params/d/SupportedCars_bmw diff --git a/opendbc_repo/opendbc/car/bmw/__init__.py b/opendbc_repo/opendbc/car/bmw/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/car/bmw/bmwcan.py b/opendbc_repo/opendbc/car/bmw/bmwcan.py new file mode 100644 index 0000000000..bf0a9f2e9f --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/bmwcan.py @@ -0,0 +1,73 @@ +from enum import Enum +from opendbc.can.packer import CANPacker +from opendbc.car.bmw.values import CanBus + +class SteeringModes(Enum): + Off = 0 + TorqueControl = 1 + AngleControl = 2 + SoftOff = 3 + +class CruiseStalk(Enum): + plus1 = "plus1" + plus5 = "plus5" + minus1 = "minus1" + minus5 = "minus5" + cancel = "cancel" + resume = "resume" + cancel_lever_up = "cancel_lever_up" + +# *** StepperServoCAN *** +def create_steer_command(frame: int, mode: SteeringModes, steer_tq: float = 0, steer_delta: float = 0): + """Creates a CAN message for the actuator STEERING_COMMAND""" + packer = CANPacker('ocelot_controls') + values = { + "COUNTER": frame % 16, + "STEER_MODE": mode.value, + "STEER_ANGLE": steer_delta, + "STEER_TORQUE": steer_tq, + } + msg = packer.make_can_msg("STEERING_COMMAND", 0, values) + addr = msg[0] + dat = msg[1] + values["CHECKSUM"] = calc_checksum_8bit(dat, addr) + + return packer.make_can_msg("STEERING_COMMAND", CanBus.SERVO_CAN, values) + + +def calc_checksum_4bit(work_data: bytearray, msg_id: int): # 0x130 + checksum = msg_id + for byte in work_data: #checksum is stripped from the dat + checksum += byte #add up all the bytes + + checksum = (checksum & 0xFF) + (checksum >> 8) #add upper and lower Bytes + checksum &= 0xFF #throw away anything in upper Byte + + checksum = (checksum & 0xF) + (checksum >> 4) #add first and second nibble + checksum &= 0xF #throw away anything in upper nibble + return checksum + +def calc_checksum_8bit(work_data: bytearray, msg_id: int): # 0xb8 0x1a0 0x19e 0xaa 0xbf + checksum = msg_id + for byte in work_data: #checksum is stripped from the data + checksum += byte #add up all the bytes + + checksum = (checksum & 0xFF) + (checksum >> 8) #add upper and lower Bytes + checksum &= 0xFF #throw away anything in upper Byte + return checksum + +def calc_checksum_cruise(work_data: bytearray):# 0x194 this checksum is special - initialized with 0 + return calc_checksum_8bit(work_data, 0) + + +def create_accel_command(packer, action: CruiseStalk, bus: int, cnt): + values = { + + } + values[action.value] = 1 + + dat = packer.make_can_msg("CruiseControlStalk", bus, values)[1] + values["Checksum_0x194"] = calc_checksum_cruise(dat) + + return packer.make_can_msg("CruiseControlStalk", bus, values) + diff --git a/opendbc_repo/opendbc/car/bmw/carcontroller.py b/opendbc_repo/opendbc/car/bmw/carcontroller.py new file mode 100644 index 0000000000..e39291344d --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/carcontroller.py @@ -0,0 +1,197 @@ +from cereal import car as car_capnp +from opendbc.car import Bus, DT_CTRL, apply_dist_to_meas_limits, apply_hysteresis +from opendbc.car.bmw import bmwcan +from opendbc.car.bmw.bmwcan import SteeringModes, CruiseStalk +from opendbc.car.bmw.values import CarControllerParams, CanBus, BmwFlags +from opendbc.car.interfaces import CarControllerBase +from opendbc.can.packer import CANPacker +from opendbc.car.common.conversions import Conversions as CV + +VisualAlert = car_capnp.CarControl.HUDControl.VisualAlert + +# DO NOT CHANGE: Cruise control step size +CC_STEP = 1 # cruise single click jump - always 1 - interpreted as km or miles depending on DSC or DME set units +CRUISE_STALK_IDLE_TICK_STOCK = 0.2 # stock cruise stalk CAN frequency when stalk is not pressed is 5Hz +CRUISE_STALK_HOLD_TICK_STOCK = 0.05 # stock cruise stalk CAN frequency when stalk is pressed is 20Hz + +CRUISE_STALK_SINGLE_TICK = CRUISE_STALK_IDLE_TICK_STOCK # we will send also at 5Hz in between stock messages to emulate single presses +CRUISE_STALK_HOLD_TICK = 0.01 # emulate held stalk, 100Hz makes stock messages be ignored + +CRUISE_SPEED_HYST_GAP = CC_STEP * 0.6 # between >0.5 and <1 to avoid cruise speed toggling. More than 0.5 to add some phase lead +ACCEL_HYST_GAP = 0.05 # m/s^2 + +ACCEL_HOLD_MEDIUM = 0.4 +DECEL_HOLD_MEDIUM = -0.6 +ACCEL_HOLD_STRONG = 1.2 +DECEL_HOLD_STRONG = -1.2 + +class CarController(CarControllerBase): + def __init__(self, dbc_names, CP): + super().__init__(dbc_names, CP) + self.flags = CP.flags + self.min_cruise_speed = CP.minEnableSpeed + self.cruise_units = None + + self.cruise_cancel = False # local cruise control cancel + self.cruise_enabled_prev = False + # redundant safety check with the board + self.apply_steer_last = 0 + self.last_cruise_rx_timestamp = 0 # stock cruise buttons + self.last_cruise_tx_timestamp = 0 # openpilot commands + self.tx_cruise_stalk_counter_last = 0 + self.rx_cruise_stalk_counter_last = -1 + self.cruise_speed_with_hyst = 0 + self.accel_with_hyst = 0 + self.accel_with_hyst_last = 0 + self.calc_desired_speed = 0 + + self.cruise_bus = CanBus.PT_CAN + if CP.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL: + self.cruise_bus = CanBus.F_CAN + + + self.packer = CANPacker(dbc_names[Bus.pt]) + + + def update(self, CC, CS, now_nanos): + + actuators = CC.actuators + can_sends = [] + + self.cruise_units = (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + + + # *** hysteresis - trend is your friend *** + # avoids cruise speed toggling and biases next request toward the direction of the previous one + self.cruise_speed_with_hyst = apply_hysteresis(CS.out.cruiseState.speed, self.cruise_speed_with_hyst, CRUISE_SPEED_HYST_GAP / self.cruise_units) + if not CS.out.cruiseState.enabled: + self.cruise_speed_with_hyst = CS.out.vEgoCluster + + # acceleration target hysteresis - avoids entering / leaving hold stalk emulation to frequently, etc + self.accel_with_hyst = apply_hysteresis(actuators.accel, self.accel_with_hyst, ACCEL_HYST_GAP) + + + # *** desired speed model *** + # detect filtered acceleration sign change and reset speed calc on change + accel_zero_cross = self.accel_with_hyst * self.accel_with_hyst_last < 0 + self.accel_with_hyst_last = self.accel_with_hyst + if accel_zero_cross or not CC.enabled or CS.out.gasPressed: + self.calc_desired_speed = CS.out.vEgoCluster + self.calc_desired_speed = self.calc_desired_speed + actuators.accel * DT_CTRL + speed_err_req = (self.calc_desired_speed - self.cruise_speed_with_hyst) * self.cruise_units + speed_err_act = self.calc_desired_speed - CS.out.vEgoCluster + + # detect incoming CruiseControlStalk message by observing counter change (message arrives at only 5Hz when nothing pressed) + if CS.cruise_stalk_counter != self.rx_cruise_stalk_counter_last: + self.tx_cruise_stalk_counter_last = CS.cruise_stalk_counter + # stock message was sent some time in between control samples: + self.last_cruise_rx_timestamp = now_nanos + self.rx_cruise_stalk_counter_last = CS.cruise_stalk_counter + + + + + + # *** send cruise control stalk message at different rates and manage counters *** + def cruise_cmd(cmd, hold=False): + time_since_cruise_sent = (now_nanos - self.last_cruise_tx_timestamp) / 1e9 + DT_CTRL / 10 # add half task sample time to account for latency + time_since_cruise_received = (now_nanos - self.last_cruise_rx_timestamp) / 1e9 + DT_CTRL / 10 # add half task sample time to account for latency + # send single cmd with an effective rate slower than held stalk rate + if not hold: + send = time_since_cruise_sent > CRUISE_STALK_SINGLE_TICK \ + and time_since_cruise_received > CRUISE_STALK_HOLD_TICK_STOCK/2 - DT_CTRL \ + and time_since_cruise_received < CRUISE_STALK_IDLE_TICK_STOCK/2 + DT_CTRL + else: + # use faster rate to emulate held stalk. Time first message such that subsequent one will nullify stock message: + send = hold and time_since_cruise_sent > CRUISE_STALK_HOLD_TICK + if send: + tx_cruise_stalk_counter = self.tx_cruise_stalk_counter_last + 1 + # avoid counter clash with a potential upcoming message from stock cruise + if tx_cruise_stalk_counter == CS.cruise_stalk_counter + 1: + # avoid clashing with upcoming stock message + # sometimes upcoming stock message is overshadowed by us, so also avoid clashing with one after that + tx_cruise_stalk_counter = tx_cruise_stalk_counter + 2 + tx_cruise_stalk_counter = tx_cruise_stalk_counter % 0xF + #can_sends.append(bmwcan.create_accel_command(self.packer, cmd, self.cruise_bus, tx_cruise_stalk_counter)) + self.tx_cruise_stalk_counter_last = tx_cruise_stalk_counter + self.last_cruise_tx_timestamp = now_nanos + + # *** cruise control cancel signal *** + # CC.cruiseControl.cancel can't be used because it is always false because pcmCruise = False because we need OP speed tracker + # CC.enabled appears after cruiseState.enabled, so we need to check rising edge to prevent instantaneous cancel after cruise is enabled + # This is because CC.enabled comes from controld and CS.out.cruiseState.enabled is from card threads + if not CC.enabled and self.cruise_enabled_prev: + self.cruise_cancel = True + # if we need to go below cruise speed, request cancel and coast while steering turns off softly + if (CS.out.cruiseState.speedCluster - self.min_cruise_speed) < 0.1 and actuators.accel < -0.1 \ + and speed_err_act < -1 and CS.out.vEgoCluster - self.min_cruise_speed < 0.4: + self.cruise_cancel = True + # keep requesting cancel until the cruise is disabled + if not CS.out.cruiseState.enabled: + self.cruise_cancel = False + + cruise_stalk_human_pressing = CS.cruise_stalk_resume or CS.cruise_stalk_cancel or CS.cruise_stalk_speed != 0 + + if not cruise_stalk_human_pressing and CS.out.cruiseState.enabled: + if self.cruise_cancel: + cruise_cmd(CruiseStalk.cancel) + print("cancel") + elif CC.enabled: + if (self.accel_with_hyst > ACCEL_HOLD_STRONG or (self.accel_with_hyst > ACCEL_HOLD_MEDIUM and speed_err_act > 3.0)) \ + and not speed_err_req < -10*CV.KPH_TO_MS*self.cruise_units: + cruise_cmd(CruiseStalk.plus5, hold=True) # produces up to 1.2 m/s2 + elif (self.accel_with_hyst < DECEL_HOLD_STRONG or (self.accel_with_hyst < DECEL_HOLD_MEDIUM and speed_err_act < -3.0)) \ + and not speed_err_req > 10*CV.KPH_TO_MS*self.cruise_units and not CS.out.gasPressed: + cruise_cmd(CruiseStalk.minus5, hold=True) # produces down to -1.4 m/s2 + elif (self.accel_with_hyst > ACCEL_HOLD_MEDIUM or speed_err_act > 2.0) \ + and not speed_err_req < -5*CV.KPH_TO_MS*self.cruise_units: + cruise_cmd(CruiseStalk.plus1, hold=True) # produces up to 0.8 m/s2 + elif (self.accel_with_hyst < DECEL_HOLD_MEDIUM or speed_err_act < -2.0) \ + and not speed_err_req > 5*CV.KPH_TO_MS*self.cruise_units and not CS.out.gasPressed: + cruise_cmd(CruiseStalk.minus1, hold=True) # produces down to -0.8 m/s2 + elif speed_err_req > max(CC_STEP/2, 0.9*CV.KPH_TO_MS*self.cruise_units) and (self.accel_with_hyst > 0.0 or CS.out.gasPressed): + cruise_cmd(CruiseStalk.plus1) + elif speed_err_req < -max(CC_STEP/2, 0.9*CV.KPH_TO_MS*self.cruise_units) and self.accel_with_hyst < 0.0 and not CS.out.gasPressed: + cruise_cmd(CruiseStalk.minus1) + + + + if self.flags & BmwFlags.STEPPER_SERVO_CAN: + steer_error = not CC.latActive and CC.enabled + if not steer_error: # don't send steer CAN tx if steering is unavailable + # *** apply steering torque *** + if CC.enabled: + new_steer = actuators.torque * CarControllerParams.STEER_MAX + # Filter out small steering commands under 0.5 Nm + if abs(new_steer) < 0.0: # 0.5 Nm threshold + apply_steer = 0 + else: + # explicitly clip torque before sending on CAN + apply_steer = apply_dist_to_meas_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, + CarControllerParams.STEER_DELTA_UP, CarControllerParams.STEER_DELTA_DOWN, + CarControllerParams.STEER_ERROR_MAX, CarControllerParams.STEER_MAX) + can_sends.append(bmwcan.create_steer_command(self.frame, SteeringModes.TorqueControl, apply_steer)) + elif not CS.cruise_stalk_cancel and not CS.out.brakePressed and not CS.out.gasPressed and self.apply_steer_last != 0: + can_sends.append(bmwcan.create_steer_command(self.frame, SteeringModes.SoftOff, self.apply_steer_last)) + apply_steer = CS.out.steeringTorqueEps + else: + apply_steer = 0 + can_sends.append(bmwcan.create_steer_command(self.frame, SteeringModes.Off)) + self.apply_steer_last = apply_steer + + # debug + if CC.enabled and (self.frame % 10) == 0: #slow print + frame_number = self.frame + print(f"Steering torque req: {actuators.torque}, Speed: {CS.out.vEgoCluster}, Frame number: {frame_number}") + + self.cruise_enabled_prev = CC.enabled + + new_actuators = actuators.as_builder() + new_actuators.torque = self.apply_steer_last / CarControllerParams.STEER_MAX + new_actuators.torqueOutputCan = self.apply_steer_last + + new_actuators.speed = self.calc_desired_speed + new_actuators.accel = speed_err_req + + self.frame += 1 + return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/bmw/carstate.py b/opendbc_repo/opendbc/car/bmw/carstate.py new file mode 100644 index 0000000000..81fd3f237d --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/carstate.py @@ -0,0 +1,207 @@ +from cereal import car as car_capnp +from opendbc.can import CANDefine, CANParser +from opendbc.car import Bus, structs +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.interfaces import CarStateBase +from opendbc.car.bmw.values import DBC, CanBus, BmwFlags, CruiseSettings + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.shifter_values = can_define.dv["TransmissionDataDisplay"]['ShiftLeverPosition'] + self.steer_angle_delta = 0. + self.gas_kickdown = False + + self.cluster_min_speed = CruiseSettings.CLUSTER_OFFSET + + self.is_metric = None + self.cruise_stalk_speed = 0 + self.cruise_stalk_resume = False + self.cruise_stalk_cancel = False + self.cruise_stalk_cancel_up = False + self.cruise_stalk_cancel_dn = False + self.cruise_stalk_counter = 0 + self.prev_cruise_stalk_speed = 0 + self.prev_cruise_stalk_resume = self.cruise_stalk_resume + self.prev_cruise_stalk_cancel = self.cruise_stalk_cancel + + self.right_blinker_pressed = False + self.left_blinker_pressed = False + self.other_buttons = False + self.prev_gas_pressed = False + self.dtc_mode = False + + def update(self, can_parsers): + cp_PT = can_parsers[Bus.pt] + cp_F = can_parsers.get(Bus.cam) + cp_aux = can_parsers.get(Bus.adas) + # set these prev states at the beginning because they are used outside the update() + self.prev_cruise_stalk_speed = self.cruise_stalk_speed + self.prev_cruise_stalk_resume = self.cruise_stalk_resume + self.prev_cruise_stalk_cancel = self.cruise_stalk_cancel + + ret = structs.CarState() + + ret.doorOpen = False # not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'] + ret.seatbeltUnlatched = False # not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] + + ret.brakePressed = cp_PT.vl["EngineAndBrake"]['BrakePressed'] != 0 + ret.parkingBrake = False # Status_contact_handbrake not available + ret.gas = cp_PT.vl['AccPedal']["AcceleratorPedalPercentage"] + # on some cars, when cruise is engaged, half pressed pedal becomes "KickDownPressed", even without pressing kickdown end stop + ret.gasPressed = cp_PT.vl['AccPedal']["AcceleratorPedalPressed"] != 0 or cp_PT.vl['AccPedal']["KickDownPressed"] != 0 + self.gas_kickdown = cp_PT.vl['AccPedal']["KickDownPressed"] != 0 #BMW has kickdown button at the bottom of the pedal + + ret.wheelSpeeds = self.get_wheel_speeds( + cp_PT.vl["WheelSpeeds"]["Wheel_FL"], + cp_PT.vl["WheelSpeeds"]["Wheel_FR"], + cp_PT.vl["WheelSpeeds"]["Wheel_RL"], + cp_PT.vl["WheelSpeeds"]["Wheel_RR"], + ) + ret.vEgoRaw = cp_PT.vl['Speed']["VehicleSpeed"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.vEgoCluster = ret.vEgo + CruiseSettings.CLUSTER_OFFSET * CV.KPH_TO_MS + ret.standstill = not cp_PT.vl['Speed']["MovingForward"] and not cp_PT.vl['Speed']["MovingReverse"] + ret.yawRate = cp_PT.vl['Speed']["YawRate"] * CV.DEG_TO_RAD + ret.steeringAngleDeg = cp_PT.vl['SteeringWheelAngle']['SteeringPosition'] + can_gear = int(cp_PT.vl["TransmissionDataDisplay"]['ShiftLeverPosition']) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + # TurnSignals not available on bus 0 + ret.leftBlinker = False + ret.rightBlinker = False + self.right_blinker_pressed = False + self.left_blinker_pressed = False + + self.dtc_mode = cp_PT.vl['StatusDSC_KCAN']['DTC_on'] != 0 # drifty traction control ;) + + # other buttons help determine driver is paying attention in case the face is not visible + self.other_buttons = \ + cp_PT.vl["SteeringButtons"]['Volume_DOWN'] !=0 or cp_PT.vl["SteeringButtons"]['Volume_UP'] !=0 or \ + cp_PT.vl["SteeringButtons"]['Previous_down'] !=0 or cp_PT.vl["SteeringButtons"]['Next_up'] !=0 or \ + cp_PT.vl["SteeringButtons"]['VoiceControl'] !=0 or \ + self.prev_gas_pressed and not ret.gasPressed # treat gas pedal tap as a button - button events indicate driver engagement - useful if face not visible + + # E-series doesn't have torque sensor + # use Voice button or gas pedal to fake steeringPressed to confirm a lane change + ret.steeringPressed = cp_PT.vl["SteeringButtons"]['VoiceControl'] !=0 or ret.gasPressed + if ret.steeringPressed and ret.leftBlinker: + ret.steeringTorque = 1 + elif ret.steeringPressed and ret.rightBlinker: + ret.steeringTorque = -1 + else: + ret.steeringTorque = 0 + + ret.espDisabled = False # Disabled ESP check + ret.cruiseState.available = True # Always available + ret.cruiseState.nonAdaptive = False # bmw doesn't have a switch + + # Read openpilot engage status from 0x100 byte 0 + ret.cruiseState.enabled = cp_PT.vl["OpenpilotEngage"]['OpenpilotEngageStatus'] != 0 + + # Read cruise control set speed from 0x100 byte 1 (u8) + ret.cruiseState.speed = cp_PT.vl["OpenpilotEngage"]['SET_SPEED'] * CV.KPH_TO_MS + + cruise_control_stal_msg = cp_PT.vl["CruiseControlStalk"] + if self.CP.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL: + # DCC implies that cruise control is done on F-CAN + # If we are sending on F-can, we also need to read on F-can to differentiate our messages from car messages + cruise_control_stal_msg = cp_F.vl["CruiseControlStalk"] + + ret.cruiseState.speedCluster = ret.cruiseState.speed + CruiseSettings.CLUSTER_OFFSET * CV.KPH_TO_MS #For logging. Doesn't do anything with pcmCruise = False + if cruise_control_stal_msg['plus1'] != 0: + self.cruise_stalk_speed = 1 + elif cruise_control_stal_msg['minus1'] != 0: + self.cruise_stalk_speed = -1 + elif cruise_control_stal_msg['plus5'] != 0: + self.cruise_stalk_speed = 5 + elif cruise_control_stal_msg['minus5'] != 0: + self.cruise_stalk_speed = -5 + else: + self.cruise_stalk_speed = 0 + self.cruise_stalk_resume = cruise_control_stal_msg['resume'] != 0 + self.cruise_stalk_cancel = cruise_control_stal_msg['cancel'] != 0 + self.cruise_stalk_cancel_up = cruise_control_stal_msg['cancel_lever_up'] != 0 + self.cruise_stalk_counter = cruise_control_stal_msg['Counter_0x194'] + self.cruise_stalk_cancel_dn = self.cruise_stalk_cancel and not self.cruise_stalk_cancel_up + + + ret.genericToggle = self.dtc_mode + + if self.CP.flags & BmwFlags.STEPPER_SERVO_CAN: + ret.steeringTorqueEps = cp_aux.vl['STEERING_STATUS']['STEERING_TORQUE'] + self.steer_angle_delta = cp_aux.vl['STEERING_STATUS']['STEERING_ANGLE'] + ret.steerFaultTemporary = int(cp_aux.vl['STEERING_STATUS']['CONTROL_STATUS']) & 0x4 != 0 + + self.prev_gas_pressed = ret.gasPressed + return ret + + @staticmethod + def get_can_parsers(CP): + messages = [ # message, frequency + ("EngineAndBrake", 100), + ("TransmissionDataDisplay", 5), + ("AccPedal", 100), + ("Speed", 50), + ("SteeringWheelAngle", 100), + ("SteeringButtons", 0), + ("WheelSpeeds", 50), # 100 on F-CAN + ("CruiseControlStalk", 5), + ("StatusDSC_KCAN", 50), + ("TerminalStatus", 10), + ("OpenpilotEngage", 10), + ] + + if CP.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL: + messages.append(("DynamicCruiseControlStatus", 5)) + if CP.flags & BmwFlags.NORMAL_CRUISE_CONTROL: + messages.append(("CruiseControlStatus", 5)) + + return { + Bus.pt: CANParser(DBC[CP.carFingerprint]['pt'], messages, CanBus.PT_CAN), + } + + @staticmethod + def get_can_parser(CP): #PT-CAN + messages = [ # message, frequency + ("EngineAndBrake", 100), + ("TransmissionDataDisplay", 5), + ("AccPedal", 100), + ("Speed", 50), + ("SteeringWheelAngle", 100), + ("SteeringButtons", 0), + ("WheelSpeeds", 50), # 100 on F-CAN + ("CruiseControlStalk", 5), + ("StatusDSC_KCAN", 50), + ("TerminalStatus", 10), + ("OpenpilotEngage", 10), + ] + + if CP.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL: + messages.append(("DynamicCruiseControlStatus", 5)) + if CP.flags & BmwFlags.NORMAL_CRUISE_CONTROL: + messages.append(("CruiseControlStatus", 5)) + + return CANParser(DBC[CP.carFingerprint]['pt'], messages, CanBus.PT_CAN) # 0: PT-CAN + + @staticmethod # $540 vehicle option could use just PT_CAN, but $544 requires sending and receiving cruise commands on F-CAN. Use F-can. Works for both options + def get_F_can_parser(CP): + if CP.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL: + messages = [ # message, frequency + ("SteeringWheelAngle_DSC", 100), + ("CruiseControlStalk", 5), + ] + else: + messages = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], messages, CanBus.F_CAN) + + @staticmethod + def get_actuator_can_parser(CP): + if CP.flags & BmwFlags.STEPPER_SERVO_CAN: + messages = [ # message, frequency + ("STEERING_STATUS", 100), + ] + else: + messages = [] + return CANParser('ocelot_controls', messages, CanBus.SERVO_CAN) diff --git a/opendbc_repo/opendbc/car/bmw/fingerprints.py b/opendbc_repo/opendbc/car/bmw/fingerprints.py new file mode 100644 index 0000000000..9cc35df895 --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/fingerprints.py @@ -0,0 +1,35 @@ +from opendbc.car.bmw.values import CAR + +BMW_E8x_E9x_common_per_bus = { + "PT-CAN": { + 256: 8, 512: 8, 128: 5, 168: 8, 169: 8, 170: 8, 172: 8, 180: 8, 181: 8, 182: 5, 184: 8, 185: 8, 186: 8, 189:8, 191: 5, 196: 7, 200: 6, 201: 8, 205: 8, 206: 8, + 209: 8,212: 8, 266: 6, 304: 5, 309: 2, 373: 3, 404: 4, 408: 5, 414: 8, 416: 8, 418: 8, 422: 8, 436: 8, 437: 7, 438: 7, 464: 8, 466: 6, 470: 2, 481: 6, + 502: 2, 514: 2, 538: 3, 550: 5, 570: 4, 578: 5, 594: 2, 678: 2, 690: 8, 691: 5, 704: 3, 719: 2, 722: 3, 753: 3, 758: 2, 760: 8, 762: 5, 764: 7, 784: 7, + 785: 2, 797: 2, 816: 8, 818: 2, 821: 8, 823: 2, 847: 2, 884: 5, 893: 2, 896: 7, 897: 2, 904: 7, 940: 2, 945: 6, 947: 6, 948: 8, 953: 3, 958: 2, 1007: 3, + 1152: 8, 1170: 8, 1175: 8, 1176: 8, 1193: 8, 1246: 8, 1408: 8, 1426: 8, 1432: 8, 1449: 8, 1472: 8, 1494: 8, 1504: 8, 1506: 8, 1517: 8, 1522: 8, 1528: 8 + }, + "STEPPER_SERVO_CAN": { + 559: 8 + } +} + +# Minimal fingerprint with just 0x200 (512 decimal) for car recognition +BMW_MINIMAL_FINGERPRINT = { + 512: 8, # 0x200 - all bytes FF for recognition +} + +BMW_E8x_E9x_common = {k: v for d in BMW_E8x_E9x_common_per_bus.values() for k, v in d.items()} # flatten + +FINGERPRINTS = { + CAR.BMW_E82: [ + BMW_MINIMAL_FINGERPRINT, # Minimal fingerprint for quick recognition + {**BMW_E8x_E9x_common, 899: 4}, + {**BMW_E8x_E9x_common, 899: 2}, # N52 has shorter msg id 899 + ], + CAR.BMW_E90: [ + BMW_MINIMAL_FINGERPRINT, # Minimal fingerprint for quick recognition + {**BMW_E8x_E9x_common, 899: 4}, + {**BMW_E8x_E9x_common, 899: 2}, # N52 has shorter msg id 899 + {**BMW_E8x_E9x_common}, # Full fingerprint for testing + ], +} diff --git a/opendbc_repo/opendbc/car/bmw/interface.py b/opendbc_repo/opendbc/car/bmw/interface.py new file mode 100755 index 0000000000..004354db6b --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/interface.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 +import numpy as np +from cereal import log +from opendbc.car import Bus, create_button_events, structs +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car import get_safety_config +from opendbc.car.bmw.carstate import CarState +from opendbc.car.bmw.carcontroller import CarController +from opendbc.car.bmw.values import CanBus, BmwFlags, CarControllerParams +from opendbc.car.interfaces import CarInterfaceBase + +ButtonType = structs.CarState.ButtonEvent.Type +EventName = log.OnroadEvent.EventName +TransmissionType = structs.CarParams.TransmissionType +GearShifter = structs.CarState.GearShifter + +# certain driver intervention can be distinguished from maximum estimated wheel turning force +def detect_stepper_override(steer_cmd, steer_act, v_ego, centering_coeff, steer_friction_torque): + # when steering released (or lost steps), what angle will it return to + # if we are above that angle, we can detect things + release_angle = steer_friction_torque / (max(v_ego, 1) ** 2 * centering_coeff) + + override = False + margin_value = 1 + if abs(steer_cmd) > release_angle: # for higher angles we steering will not move outward by itself with stepper on + if steer_cmd > 0: + override |= steer_act - steer_cmd > margin_value # driver overrode from right to more right + override |= steer_act < 0 # releaseAngle -3 # driver overrode from right to opposite direction + else: + override |= steer_act - steer_cmd < -margin_value # driver overrode from left to more left + override |= steer_act > 0 # -releaseAngle +3 # driver overrode from left to opposite direction + # else: + # override |= abs(steerAct) > releaseAngle + marginVal # driver overrode to an angle where steering will not go by itself + return override + + +class CarInterface(CarInterfaceBase): + CarState = CarState + CarController = CarController + + def __init__(self, CP): + super().__init__(CP) + + # Add additional F-CAN and auxiliary parsers to the dictionary + self.can_parsers[Bus.cam] = self.CS.get_F_can_parser(CP) + self.can_parsers[Bus.adas] = self.CS.get_actuator_can_parser(CP) + + @staticmethod + # servotronic is a bit more lighter in general and especially at low speeds https://www.spoolstreet.com/threads/servotronic-on-a-335i.1400/page-13#post-117705 + def get_steer_feedforward_servotronic(desired_angle, v_ego): # accounts for steering rack ratio and/or caster nonlinearities https://www.spoolstreet.com/threads/servotronic-on-a-335i.1400/page-15#post-131271 + angle_bp = [-40.0, -6.0, -4.0, -3.0, -2.0, -1.0, -0.5, 0.5, 1.0, 2.0, 3.0, 4.0, 6.0, 40.0] # deg + hold_torque_v = [-6, -2.85, -2.5, -2.25, -2, -1.65, -1, 1, 1.65, 2, 2.25, 2.5, 2.85, 6] # Nm + hold_torque = np.interp(desired_angle, angle_bp, hold_torque_v) + return hold_torque # todo add speed component + + @staticmethod + def get_steer_feedforward(desired_angle, v_ego): + angle_bp = [-40.0, -6.0, -4.0, -3.0, -2.0, -1.0, -0.5, 0.5, 1.0, 2.0, 3.0, 4.0, 6.0, 40.0] # deg + hold_torque_v = [-6, -2.85, -2.5, -2.25, -2, -1.65, -1, 1, 1.65, 2, 2.25, 2.5, 2.85, 6] # N + hold_torque = np.interp(desired_angle, angle_bp, hold_torque_v) + return hold_torque # todo add speed component + + def get_steer_feedforward_function(self): + if self.CP.flags & BmwFlags.SERVOTRONIC: + return self.get_steer_feedforward_servotronic + else: + return self.get_steer_feedforward + + @staticmethod + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, is_release, docs): + # Handle empty fingerprint (when forcing BMW) + if not fingerprint: + fingerprint = {CanBus.PT_CAN: {}, CanBus.SERVO_CAN: {}, CanBus.F_CAN: {}} + ret.flags |= BmwFlags.STEPPER_SERVO_CAN.value # Always enable when forcing + + if 0x22F in fingerprint.get(CanBus.SERVO_CAN, {}): + ret.flags |= BmwFlags.STEPPER_SERVO_CAN.value + + ret.openpilotLongitudinalControl = False + ret.radarUnavailable = True + ret.pcmCruise = False # openpilot handles engagement via cruise stalk + + ret.autoResumeSng = False + if 0x200 in fingerprint.get(CanBus.PT_CAN, {}): # Enigne controls speed and reports cruise control status + ret.flags |= BmwFlags.NORMAL_CRUISE_CONTROL.value # openpilot will inject cruise stalk +/- requests + elif 0x193 in fingerprint.get(CanBus.PT_CAN, {}): # either DSC or LDM reports cruise control status + if 0x0D5 not in fingerprint.get(CanBus.PT_CAN, {}): # DSC itself applies brakes + ret.flags |= BmwFlags.DYNAMIC_CRUISE_CONTROL.value # openpilot will inject cruise stalk +/- requests on F-CAN + else: # LDM sends brake commands + ret.flags |= BmwFlags.ACTIVE_CRUISE_CONTROL_NO_ACC.value # openpilot will switch between OP and LDM + ret.autoResumeSng = True #! hopefully + else: # DSC/DME not sending cruise status and LDM not present - openpilot will be the only requester + ret.flags |= BmwFlags.ACTIVE_CRUISE_CONTROL_NO_LDM.value + ret.autoResumeSng = True #! hopefully + + if 0xb8 in fingerprint.get(CanBus.PT_CAN, {}) or 0xb5 in fingerprint.get(CanBus.PT_CAN, {}): # transmission: engine torque requests + ret.transmissionType = TransmissionType.automatic + else: + ret.transmissionType = TransmissionType.manual + + # Detect all wheel drive BMW E90 XI + if 0xbc in fingerprint.get(CanBus.PT_CAN, {}): # XI has a transfer case + ret.steerRatio = 18.5 # XI has slower steering rack + + if ret.flags & BmwFlags.DYNAMIC_CRUISE_CONTROL: # DCC imperial has higher threshold + ret.minEnableSpeed = 0. * CV.KPH_TO_MS # if self.CS.is_metric else 20. * CV.MPH_TO_MS + if ret.flags & BmwFlags.NORMAL_CRUISE_CONTROL: + ret.minEnableSpeed = 0. * CV.KPH_TO_MS + + ret.brand = "bmw" + ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.allOutput)] + + ret.steerControlType = structs.CarParams.SteerControlType.torque + ret.steerActuatorDelay = 0.2 + ret.steerLimitTimer = 0.4 + + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + ret.lateralTuning.torque.kp = 8.5 / CarControllerParams.STEER_MAX + ret.lateralTuning.torque.ki = 3.5 / CarControllerParams.STEER_MAX + ret.lateralTuning.torque.kf = 2.1 / CarControllerParams.STEER_MAX + ret.lateralTuning.torque.useSteeringAngle = True + ret.lateralTuning.torque.steeringAngleDeadzoneDeg = 0.0 # backlash of stepper? + + ret.longitudinalActuatorDelay = 1.0 #s, Gas/Brake actuator delay + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [1.0] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.1] + + ret.centerToFront = ret.wheelbase * 0.44 + + ret.startAccel = 0.0 + print("Controller: " + ret.lateralTuning.which()) + + # has_servotronic = False + # for fw in car_fw: # todo check JBBF firmware for $216A + # if fw.ecu == "eps" and b"," in fw.fwVersion: + # has_servotronic = True + + return ret + + def _update(self, c): + # ******************* do can recv ******************* + ret = self.CS.update(self.cp, self.cp_F, self.cp_aux) + + # events + events = self.create_common_events(ret, pcm_enable=False) + + # *** cruise control units detection *** + # when cruise is enabled the car sets cruiseState.speed = vEgo, so we can detect the ratio + # with resume this wouldn't work, but op will not engage on first resume anyway + if self.CS.is_metric is None and c.enabled and ret.vEgo > 0: + # note, when is_metric is None, cruiseState.speed is already scaled by CV.MPH_TO_MS by default + speed_ratio = ret.cruiseState.speed / ret.vEgo # 1 if imperial, 1.6 if metric + if 0.8 < speed_ratio < 1.2: + self.CS.is_metric = False + elif 0.8 * CV.MPH_TO_KPH < speed_ratio < 1.2 * CV.MPH_TO_KPH: + self.CS.is_metric = True + else: + events.add(EventName.accFaulted) + + + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_stalk_speed > 0, self.CS.prev_cruise_stalk_speed > 0, {1: ButtonType.accelCruise}), + *create_button_events(self.CS.cruise_stalk_speed < 0, self.CS.prev_cruise_stalk_speed < 0, {1: ButtonType.decelCruise}), + *create_button_events(self.CS.cruise_stalk_cancel, self.CS.prev_cruise_stalk_cancel, {1: ButtonType.cancel}), + *create_button_events(self.CS.other_buttons, not self.CS.other_buttons, {1: ButtonType.altButton1}), + *create_button_events(self.CS.cruise_stalk_resume, self.CS.prev_cruise_stalk_resume, { + 1: ButtonType.resumeCruise if not c.enabled else ButtonType.gapAdjustCruise}) # repurpose resume button to adjust driver personality when engaged + ] + + if ret.vEgoCluster < self.CP.minEnableSpeed: + events.add(EventName.belowEngageSpeed) + if c.actuators.accel > 0.2: + events.add(EventName.speedTooLow) # can't restart cruise anymore + + ret.events = events.to_msg() + + return ret diff --git a/opendbc_repo/opendbc/car/bmw/radar_interface.py b/opendbc_repo/opendbc/car/bmw/radar_interface.py new file mode 100644 index 0000000000..6e552bf618 --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/radar_interface.py @@ -0,0 +1,4 @@ +from opendbc.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/opendbc_repo/opendbc/car/bmw/tests/__init__.py b/opendbc_repo/opendbc/car/bmw/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/car/bmw/tests/test_bmw.py b/opendbc_repo/opendbc/car/bmw/tests/test_bmw.py new file mode 100644 index 0000000000..dcbe8c239d --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/tests/test_bmw.py @@ -0,0 +1,37 @@ +from parameterized import parameterized + +from openpilot.selfdrive.car.bmw.fingerprints import FINGERPRINTS + +N55_ENGINE_MSG = {899: 4} +N52_ENGINE_MSG = {899: 2} +CRUISE_STATUS_MSG = {0x200: 8} +DYNAMIC_CRUISE_STATUS_MSG = {0x193: 8} +STEPPER_MSG = {0x22F: 8} + + + + +class TestBMWFingerprint: + @parameterized.expand(FINGERPRINTS.items()) + def test_can_fingerprints(self, car_model, fingerprints): + assert len(fingerprints) > 0 + + assert all(len(finger) for finger in fingerprints) + + for car_config in ((STEPPER_MSG, N55_ENGINE_MSG, DYNAMIC_CRUISE_STATUS_MSG), + (STEPPER_MSG, N52_ENGINE_MSG, DYNAMIC_CRUISE_STATUS_MSG), + (N55_ENGINE_MSG, CRUISE_STATUS_MSG, DYNAMIC_CRUISE_STATUS_MSG), + ): + failed_fingers = {} + for i, finger in enumerate(fingerprints): + failed_addrs = [] + for msg in (car_config): + for addr, length in msg.items(): + found_length = finger.get(addr) + if found_length != length: + failed_addrs.append((addr, length, found_length)) + if failed_addrs: + failed_fingers[i] = failed_addrs + + if len(failed_fingers) == len(fingerprints): + raise AssertionError(f"All {len(fingerprints)} fingerprints failed: {failed_fingers}") diff --git a/opendbc_repo/opendbc/car/bmw/values.py b/opendbc_repo/opendbc/car/bmw/values.py new file mode 100644 index 0000000000..62e68f4fca --- /dev/null +++ b/opendbc_repo/opendbc/car/bmw/values.py @@ -0,0 +1,91 @@ +from dataclasses import dataclass, field +from enum import Enum, IntFlag +from cereal import car as car_capnp +from opendbc.car import Platforms, CarSpecs, PlatformConfig, DbcDict, STD_CARGO_KG +from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column +from opendbc.car.common.conversions import Conversions as CV + +# Steer torque limits +class CarControllerParams: #controls running @ 100hz + STEER_STEP = 1 # 100Hz + STEER_MAX = 12 # Nm + STEER_DELTA_UP = 0.1 # Nm/10ms + STEER_DELTA_DOWN = 1.0 # Nm/10ms + STEER_ERROR_MAX = 999 # max delta between torque cmd and torque motor + + # STEER_BACKLASH = 1 #deg + def __init__(self, CP): + pass + +class BmwFlags(IntFlag): + # Detected Flags + STEPPER_SERVO_CAN = 2 ** 0 + NORMAL_CRUISE_CONTROL = 2 ** 1 # CC $540 + DYNAMIC_CRUISE_CONTROL = 2 ** 2 # DCC $544 + ACTIVE_CRUISE_CONTROL = 2 ** 3 # ACC $541 - LDM and ACC sensor - #! not supported + ACTIVE_CRUISE_CONTROL_NO_ACC = 2 ** 4 # no ACC module - DSC, DME, KOMBI coded to $541, LDM coded to $544 + ACTIVE_CRUISE_CONTROL_NO_LDM = 2 ** 5 # no LDM/ACC - DSC, DME, KOMBI coded to $541 + SERVOTRONIC = 2 ** 6 # ServoTonic $216A - TODO: needs firmware query + +class CruiseSettings: + CLUSTER_OFFSET = 2 # kph + +class CanBus: + PT_CAN = 0 + SERVO_CAN = 0 # required for steering + F_CAN = 0 # required for DYNAMIC_CRUISE_CONTROL or optional for logging + K_CAN = 0 # not used - only logging + + +class Footnote(Enum): + StepperServoCAN = CarFootnote( + "Requires StepperServoCAN", + Column.FSR_STEERING) + DCC = CarFootnote( + "Minimum speed with CC or DCC is 30 kph", + Column.FSR_LONGITUDINAL) + CC = CarFootnote( + "Normal cruise control should work but was not tested in a while. Code in DCC instead or provide a fix", + Column.PACKAGE) + ACC = CarFootnote( + "ACC is required. Also LDM module to take over when OP is off.", + Column.AUTO_RESUME) + DIY = CarFootnote( + "For CC and DCC only a diy USB-C and a resistor is required or a harness box DIY connector", + Column.HARDWARE) + +@dataclass +class BmwCarDocs(CarDocs): + package: str = "Cruise Control - VO540, VO544, VO541" + footnotes: list[Enum] = field(default_factory=lambda: [Footnote.StepperServoCAN, Footnote.DCC, Footnote.CC, Footnote.ACC, Footnote.DIY]) + + def init_make(self, CP: car_capnp.CarParams): + self.car_parts = CarParts.common([CarHarness.custom]) + +@dataclass +class BmwPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: {'pt': 'bmw_e9x_e8x'}) + + +class CAR(Platforms): + BMW_E82 = BmwPlatformConfig( + [BmwCarDocs("BMW E82 2004-13")], + CarSpecs(mass=3145. * CV.LB_TO_KG + STD_CARGO_KG, wheelbase=2.66, steerRatio=16.00, tireStiffnessFactor=0.8) + ) + BMW_E90 = BmwPlatformConfig( + [BmwCarDocs("BMW E90 2005-11")], + CarSpecs(mass=3300. * CV.LB_TO_KG + STD_CARGO_KG, wheelbase=2.76, steerRatio=16.00, tireStiffnessFactor=0.8) + ) + + +DBC = CAR.create_dbc_map() + + +if __name__ == '__main__': + cars = [] + for platform in CAR: + for doc in platform.config.car_docs: + cars.append(doc.name) + cars.sort() + for c in cars: + print(c) diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index 7057c8c83d..f20cb6b770 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -244,7 +244,7 @@ struct CarState { pcmCruiseGap @63 :Int16; #0: can't read, 1,2,3,4: gap setting speedLimit @64 :Float32; speedLimitDistance @65 :Float32; - gearStep @66 :Int16; + gearStep @66 :Int16; tpms @67 : Tpms; useLaneLineSpeed @68 : Float32; leftLatDist @69 : Float32; # distance to left lane line @@ -712,6 +712,7 @@ struct CarParams { fcaGiorgio @32; rivian @33; volkswagenMeb @34; + bmw @35; } enum SteerControlType { diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index b3283d5c5a..3b4db0097f 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -21,9 +21,12 @@ def load_interfaces(brand_names): ret = {} for brand_name in brand_names: path = f'opendbc.car.{brand_name}' - CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface - for model_name in brand_names[brand_name]: - ret[model_name] = CarInterface + try: + CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface + for model_name in brand_names[brand_name]: + ret[model_name] = CarInterface + except ImportError as e: + carlog.warning(f"Failed to load interface for {brand_name}: {e}") return ret @@ -153,45 +156,24 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, alpha_long_allowed: bool, is_release: bool, num_pandas: int = 1, cached_params: CarParamsT | None = None): - candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(can_recv, can_send, set_obd_multiplexing, num_pandas, cached_params) - - if candidate is None: - carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)}) - candidate = "MOCK" - - selected_car = Params().get("CarSelected3") - if selected_car: - def find_car(name: str): - from opendbc.car.hyundai.values import CAR as HYUNDAI - from opendbc.car.gm.values import CAR as GM - from opendbc.car.toyota.values import CAR as TOYOTA - from opendbc.car.mazda.values import CAR as MAZDA - for platform in GM: - for doc in platform.config.car_docs: - if name == doc.name: - return platform - for platform in TOYOTA: - for doc in platform.config.car_docs: - if name == doc.name: - return platform - for platform in HYUNDAI: - for doc in platform.config.car_docs: - if name == doc.name: - return platform - for platform in MAZDA: - for doc in platform.config.car_docs: - if name == doc.name: - return platform - return None - found_car = find_car(selected_car) - if found_car is not None: - candidate = found_car + # Force BMW - skip fingerprinting + from opendbc.car.bmw.values import CAR as BMW + from opendbc.car.bmw.interface import CarInterface as BMWCarInterface + + candidate = BMW.BMW_E90 + fingerprints = {} + vin = "" + car_fw = [] + source = "fixed" + exact_match = False print(f"SelectedCar = {candidate}") Params().put("CarName", str(candidate)) Params().put("FingerPrints", str(fingerprints)) - CarInterface = interfaces[candidate] + + # Use BMW interface directly + CarInterface = BMWCarInterface CP: CarParams = CarInterface.get_params(candidate, fingerprints, car_fw, alpha_long_allowed, is_release, docs=False) CP.carVin = vin CP.carFw = car_fw diff --git a/opendbc_repo/opendbc/car/fingerprints.py b/opendbc_repo/opendbc/car/fingerprints.py index db140d1aed..6725fc7877 100644 --- a/opendbc_repo/opendbc/car/fingerprints.py +++ b/opendbc_repo/opendbc/car/fingerprints.py @@ -11,6 +11,7 @@ from opendbc.car.subaru.values import CAR as SUBARU from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.volkswagen.values import CAR as VW +from opendbc.car.bmw.values import CAR as BMW FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True) _FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True) diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 33868bca0e..a6269f8bb7 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -110,3 +110,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "HYUNDAI_CASPER_EV"= [2.5, 2.5, 0.1] "HYUNDAI_IONIQ_5_N" = [3.17, 2.71, 0.097] "HYUNDAI_IONIQ_5_PE" = [1.75, 1.75, 0.15] + +# BMW +"BMW_E90" = [2.3, 1.5, 0.15] diff --git a/opendbc_repo/opendbc/car/values.py b/opendbc_repo/opendbc/car/values.py index cd7dbb95ce..1f8ea031ed 100644 --- a/opendbc_repo/opendbc/car/values.py +++ b/opendbc_repo/opendbc/car/values.py @@ -13,8 +13,10 @@ from opendbc.car.tesla.values import CAR as TESLA from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN +from opendbc.car.bmw.values import CAR as BMW + +Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN | BMW -Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN BRANDS = get_args(Platform) PLATFORMS: dict[str, Platform] = {str(platform): platform for brand in BRANDS for platform in brand} diff --git a/opendbc_repo/opendbc/dbc/bmw_e9x_e8x.dbc b/opendbc_repo/opendbc/dbc/bmw_e9x_e8x.dbc index 7ec95c0b19..2529f2803c 100644 --- a/opendbc_repo/opendbc/dbc/bmw_e9x_e8x.dbc +++ b/opendbc_repo/opendbc/dbc/bmw_e9x_e8x.dbc @@ -1,6 +1,5 @@ VERSION "" - NS_ : NS_DESC_ CM_ @@ -33,344 +32,150 @@ NS_ : BS_: -BU_: EON XXX RDC SZL VGSG JBBF RFK FLA RAD1 CAS CID AHM HKL HUD EKP DWA DSC SM_BF GWS VDM DDE1 ACI CCC DSC SM_FA CTM LDM RSE MRSZ VDA EDCK ZBE EGS ACC_Sensor Kombi IHKA ARS ACSM FZD PGS NVC AFS DME FRMFA EMF FKA VSW EPS PDC DKG EHC Diagnosetool_PT_CAN Diagnosetool_K_CAN_System Vector__XXX +BU_: EON RDC SZL VGSG JBBF RFK FLA RAD1 CAS CID AHM HKL HUD EKP DWA DSC SM_BF GWS VDM DDE1 ACI CCC SM_FA CTM LDM RSE MRSZ VDA EDCK ZBE EGS ACC Kombi IHKA ARS ACSM FZD PGS NVC AFS DME FRMFA EMF FKA VSW EPS PDC DKG SMG EHC SZM KGM Diagnosetool_PT_CAN Diagnosetool_K_CAN_System Vector__XXX XXX +CM_ "Source "; +CM_ "Tool32 reference https://www.bimmerforums.com/forum/showthread.php?2298830-E90-Can-bus-project-(E60-E65-E87-)&p=29628499#post29628499" +CM_ "License MIT"; -BO_ 170 AccPedal: 8 DME - SG_ KickDownPressed : 53|1@0+ (1,0) [0|3] "" XXX - SG_ CruisePedalActive : 54|1@0+ (1,0) [0|1] "" XXX - SG_ CruisePedalInactive : 55|1@0+ (1,0) [0|1] "" XXX - SG_ ThrottlelPressed : 50|1@0+ (1,0) [0|1] "" XXX - SG_ AcceleratorPedalPressed : 52|1@0+ (1,0) [0|7] "" XXX - SG_ AcceleratorPedalPercentage : 16|16@1+ (0.04,0) [0|100] "" XXX - SG_ Counter_170 : 8|4@1+ (1,0) [0|15] "" XXX - SG_ EngineSpeed : 32|16@1+ (0.25,0) [0|8000] "rpm" XXX - SG_ Checksum_170 : 0|8@1- (1,0) [0|65535] "" XXX - -BO_ 404 CruiseControl: 4 SZL - SG_ plus1mph_request : 16|1@0+ (1,0) [0|1] "" XXX - SG_ minus1mph_request : 18|1@0+ (1,0) [0|1] "" XXX - SG_ Cancel_request_up_stalk : 23|1@0+ (1,0) [0|1] "" XXX - SG_ Cancel_request_up_or_down_stalk : 20|1@0+ (1,0) [0|1] "" XXX - SG_ Resume_request : 22|1@0+ (1,0) [0|1] "" XXX - SG_ setMe_0xFC : 31|8@0+ (1,0) [0|255] "" XXX - SG_ plus5mph_request : 17|1@0+ (1,0) [0|0] "" Vector__XXX - SG_ minus5mph_request : 19|1@0+ (1,0) [0|0] "" Vector__XXX - SG_ requests_0xF : 15|4@0+ (1,0) [0|15] "" XXX - SG_ Counter_404 : 11|4@0+ (1,0) [0|15] "" XXX - SG_ Checksum_404 : 7|8@0+ (1,0) [0|15] "" XXX +BO_ 128 SYNC: 5 XXX + SG_ State1 : 16|8@1+ (1,0) [0|255] "" XXX + SG_ State2 : 24|4@1+ (1,0) [0|15] "" XXX + SG_ Counter_128 : 28|4@1+ (1,0) [0|15] "" XXX -BO_ 512 CruiseControlStatus: 8 DME - SG_ CruiseControlInactiveFlag : 12|1@0+ (1,0) [0|1] "" XXX - SG_ CruiseCoontrolActiveFlag : 13|1@0+ (1,0) [0|1] "" XXX - SG_ CruiseControlSetpointSpeed : 7|8@0+ (0.25,0) [0|255] "mph" XXX +BO_ 133 Synchronization_SC_VDA: 8 DSC BO_ 168 EngineAndBrake: 8 DME - SG_ Checksum_EngineAndBrake : 0|8@1+ (1,0) [0|0] "" XXX - SG_ BrakePressed : 61|1@0+ (1,0) [0|1] "" XXX - SG_ Brake_active2 : 62|1@0+ (1,0) [0|1] "" XXX - SG_ ST_RCPT_ENG_DSC : 52|2@1+ (1,0) [0|0] "" XXX - SG_ ST_RCPT_ENG_ARS : 50|2@1+ (1,0) [0|0] "" XXX - SG_ ST_RCPT_ENG_ACC : 48|2@1+ (1,0) [0|0] "" XXX - SG_ ST_RCPT_ENG_EGS : 54|2@1+ (1,0) [0|0] "" XXX - SG_ ST_DMEA_SWO : 44|2@1+ (1,0) [0|0] "" XXX + SG_ Checksum_0xa8 : 0|8@1+ (1,0) [0|0] "" XXX + SG_ Counter_0xa8 : 8|4@1+ (1,0) [0|15] "" XXX SG_ EngineTorque : 12|12@1- (0.03125,0) [-1024|1023] "" XXX - SG_ ALIV_TORQ_1_DME : 8|4@1+ (1,0) [0|15] "" XXX SG_ EngineTorqueWoInterv : 24|16@1- (0.03125,0) [-1024|1023.96875] "" XXX - -BO_ 470 SteeringButtons: 2 SZL - SG_ Volume_DOWN : 2|1@0+ (1,0) [0|1] "" XXX - SG_ Volume_UP : 3|1@0+ (1,0) [0|1] "" XXX - SG_ VoiceControl : 8|1@0+ (1,0) [0|1] "" XXX - SG_ Telephone : 0|1@0+ (1,0) [0|1] "" XXX - SG_ Next_up : 5|1@0+ (1,0) [0|1] "" XXX - SG_ Previous_down : 4|1@0+ (1,0) [0|1] "" XXX - -BO_ 403 DynamicCruiseControlStatus: 8 DSC - SG_ Counter_403 : 7|8@0+ (1,0) [0|255] "" XXX - SG_ CruiseActive : 43|1@0+ (1,0) [0|1] "" XXX - SG_ CruiseSpeedChangeRequest : 48|1@0+ (1,0) [0|1] "" XXX - SG_ CruiseControlSetpointSpeed : 15|8@0+ (1,-2) [0|255] "kph/mph" XXX - -BO_ 201 SteeringWheelAngle_DSC: 8 SZL - SG_ Counter_201 : 20|4@1+ (1,0) [0|15] "" DSC - SG_ SteeringPositionComplementLow : 24|11@1- (1,0) [0|1] "" DSC - SG_ SteeringPosition : 0|16@1- (0.0428316886,0) [-600|600] "deg" DSC - -BO_ 206 WheelSpeeds: 8 DSC - SG_ Wheel_FL : 0|16@1- (0.0625,0) [0|255] "kph" XXX - SG_ Wheel_FR : 16|16@1- (0.0625,0) [0|255] "kph" XXX - SG_ Wheel_RL : 32|16@1- (0.0625,0) [0|255] "kph" XXX - SG_ Wheel_RR : 48|16@1- (0.0625,0) [0|255] "kph" XXX - -BO_ 884 WheelToleranceAdjustment: 8 DSC - -BO_ 678 WiperSwitch: 8 SZL - SG_ AutoWipersOn : 0|1@1+ (1,0) [0|3] "" XXX - -BO_ 304 TerminalStatus: 8 CAS - SG_ AccOn : 23|1@1+ (1,0) [0|255] "" XXX - SG_ IgnitionOff : 22|1@1+ (1,0) [0|3] "" XXX - SG_ Counter_304 : 32|4@1+ (1,0) [0|15] "" XXX - SG_ Checksum_304 : 36|4@1+ (1,0) [0|255] "" XXX + SG_ ST_DMEA_SWO : 44|2@1+ (1,0) [0|0] "" XXX + SG_ ST_RCPT_ENG_ACC : 48|2@1+ (1,0) [0|0] "" XXX + SG_ ST_RCPT_ENG_ARS : 50|2@1+ (1,0) [0|0] "" XXX + SG_ ST_RCPT_ENG_DSC : 52|2@1+ (1,0) [0|0] "" XXX + SG_ ST_RCPT_ENG_EGS : 54|2@1+ (1,0) [0|0] "" XXX + SG_ BrakePressed : 61|1@0+ (1,0) [0|1] "" XXX + SG_ BrakeActive : 62|1@0+ (1,0) [0|1] "" XXX BO_ 169 Torque2: 8 DME - SG_ TORQ_AVL_SPAR_POS : 52|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX - SG_ TORQ_AVL_SPAR_NEG : 40|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX - SG_ TORQ_AVL_MAX : 28|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX - SG_ TORQ_AVL_MIN : 16|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX - SG_ ST_INFS : 14|2@1+ (1,0) [0|0] "" XXX + SG_ Checksum_0xa9 : 0|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ Counter_0xa9 : 8|4@1+ (1,0) [0|0] "" XXX SG_ ST_SW_LEV_RPM : 12|2@1+ (1,0) [0|0] "" XXX - SG_ ALIV_TORQ_2_DME : 8|4@1+ (1,0) [0|0] "" XXX - SG_ CHKSM_TORQ_2_DME : 0|8@1+ (1,0) [0|0] "" Vector__XXX - -BO_ 184 TorqueTransmisionRequest: 8 LDM - SG_ Checksum_184 : 0|8@1+ (1,0) [0|15] "" XXX - SG_ Counter_184 : 8|4@1+ (1,0) [0|15] "" XXX - -BO_ 196 SteeringWheelAngle: 7 DSC - SG_ SteeringSpeed : 24|16@1- (0.0428316886,0) [0|255] "degree/s" XXX - SG_ SteeringPosition : 0|16@1- (0.0428316886,0) [-600|600] "degree" XXX - -BO_ 180 WheelTorqueDriveTrain1: 8 DME - -BO_ 182 DynamicCruiseControlTorqueDemand: 8 DSC - SG_ TORQ_TAR_DSC : 12|12@1- (0.5,0) [0|1000] "" XXX - SG_ Counter_182 : 8|4@1+ (1,0) [0|14] "" XXX - SG_ Checksum_182 : 0|8@1+ (1,0) [0|15] "" XXX - -BO_ 186 TransmissionData: 8 EGS - SG_ Counter_186 : 48|4@1+ (1,0) [0|14] "" XXX - SG_ Shifting : 4|1@1+ (1,0) [0|15] "" XXX - SG_ OutputShaftSpeed : 24|16@1- (0.125,0) [0|255] "rpm" XXX - SG_ GearRatio : 8|8@1+ (0.05,0) [0|255] "" XXX - SG_ GearTar : 0|4@1+ (1,-4) [0|255] "" XXX - SG_ Checksum_186 : 40|8@1+ (1,0) [0|15] "" XXX - -BO_ 191 RequestedWheelTorqueDriveTrain: 8 LDM - SG_ Checksum_191 : 0|8@1+ (1,0) [0|19] "" XXX - SG_ Counter_191 : 8|4@1- (1,0) [0|255] "" XXX - SG_ TorqueReq : 16|12@1- (0.5,350) [-1024|1023.96875] "" XXX - -BO_ 414 StatusDSC_KCAN: 8 DSC - SG_ BrakePressure : 48|8@1- (1,0) [0|255] "" XXX - SG_ BrakeStates : 40|8@1+ (1,0) [0|255] "" XXX - SG_ Checksum_414 : 56|8@1+ (1,0) [0|15] "" XXX - SG_ Counter_414 : 20|4@1+ (1,0) [0|15] "" XXX - SG_ DTC_on : 12|1@1+ (1,0) [0|3] "" XXX - SG_ DSC_full_off : 8|4@1+ (1,0) [0|15] "" XXX - -BO_ 416 Speed: 8 DSC - SG_ AccX : 28|12@1- (1,0) [0|15] "" XXX - SG_ YawRate : 40|12@1- (1,0) [0|255] "" XXX - SG_ VehicleSpeed : 0|12@1- (0.103,0) [0|255] "kph" XXX - SG_ MovingReverse : 13|1@1+ (1,0) [0|3] "" XXX - SG_ AccY : 16|12@1- (1,0) [0|255] "" XXX - SG_ Counter_416 : 52|4@1+ (1,0) [0|14] "" XXX - SG_ Checksum_416 : 56|8@1+ (1,0) [0|15] "" XXX - SG_ MovingForward : 12|1@1+ (1,0) [0|15] "" XXX - -BO_ 418 TransimissionData2: 8 EGS - SG_ ManualMode : 50|1@0+ (1,0) [0|255] "" XXX - SG_ Counter_418 : 28|4@1+ (1,0) [0|14] "" XXX - SG_ Checksum_418 : 56|8@1+ (1,0) [0|15] "" XXX - -BO_ 690 WheelPressure_KCAN: 8 DSC - -BO_ 691 AccelerationData: 8 DSC - -BO_ 402 GearSelectorSwitch_1: 4 XXX - SG_ Counter_402 : 24|4@1+ (1,0) [0|14] "" XXX - -BO_ 408 GearSelectorSwitch: 8 GWS - SG_ ParkButtonSecond : 26|2@1+ (1,0) [0|3] "" XXX - SG_ SideButton : 28|2@1+ (1,0) [0|3] "" XXX - SG_ SportButtonPressed : 34|2@1+ (1,0) [0|255] "" XXX - SG_ M3_button : 36|2@1+ (1,0) [0|3] "" XXX - SG_ SideButtonXOR11 : 30|2@1+ (1,0) [0|3] "" XXX - SG_ param1XOR11 : 22|2@1+ (1,0) [0|3] "" XXX - SG_ m3ShifterPositionXOR1111 : 8|4@1+ (1,0) [0|15] "" XXX - SG_ always11 : 38|2@1+ (1,0) [0|3] "" XXX - SG_ m3ShifterPosition : 4|4@1+ (1,0) [0|15] "" XXX - SG_ param1 : 20|2@1+ (1,0) [0|3] "" XXX - SG_ param5 : 32|2@1+ (1,0) [0|3] "" XXX - SG_ Counter_408 : 0|4@1+ (1,0) [0|14] "" XXX - SG_ ParkButtonFirst : 24|2@1+ (1,0) [0|3] "" XXX - SG_ ShifterPositionXOR1111 : 16|4@1+ (1,0) [0|15] "" XXX - SG_ ShifterPosition : 12|4@1+ (1,0) [0|0] "" Vector__XXX - -BO_ 422 DistanceRoute: 8 DSC - -BO_ 436 InstrumentClusterStatus_KOMBI: 8 CCC - SG_ HandbrakeActive : 41|1@1+ (1,0) [0|3] "" XXX - -BO_ 464 EngineData: 8 DME - SG_ RPM_IDLG_TAR : 56|8@1+ (5,0) [0|1270] "1/min" XXX - SG_ CTR_SLCK : 48|2@1+ (1,0) [0|0] "" XXX - SG_ IJV_FU : 32|16@1+ (1,-48) [0|0] "C" XXX - SG_ AIP_ENG : 24|8@1+ (2,598) [600|1106] "hPa" XXX - SG_ ST_SW_WAUP : 22|2@1+ (1,0) [0|0] "" XXX - SG_ ST_ENG_RUN : 20|2@1+ (1,0) [0|0] "" XXX - SG_ Counter_464 : 16|4@1+ (1,0) [0|14] "" XXX - SG_ TEMP_EOI : 8|8@1+ (1,-48) [0|0] "C" XXX - SG_ TEMP_ENG : 0|8@1+ (1,-48) [0|0] "C" XXX - -BO_ 945 TransmissionData3: 8 DKG - SG_ Checksum_946 : 0|8@1+ (1,0) [0|19] "" XXX - SG_ Counter_945 : 8|4@1+ (1,0) [0|14] "" XXX - -BO_ 200 SteeringWheelAngle_slow: 6 SZL - SG_ SteeringPosition : 0|16@1- (0.0428316886,0) [-600|600] "degree" XXX - SG_ SteeringSpeed : 24|16@1- (0.0428316886,0) [-65535|65535] "degree/s" XXX - SG_ Counter_200 : 20|4@1+ (1,0) [0|15] "" XXX - -BO_ 466 TransmissionDataDisplay: 8 EGS - SG_ ShiftLeverMode : 32|2@1+ (1,0) [0|3] "" XXX - SG_ GearAct : 12|4@1+ (1,-4) [0|15] "" XXX - SG_ Counter_466 : 28|4@1+ (1,0) [0|14] "" XXX - SG_ ShiftLeverPosition : 0|4@1+ (1,0) [0|8] "" XXX - SG_ xFF : 40|8@1+ (1,0) [0|255] "" XXX - SG_ ShiftLeverPositionXOR : 4|4@1+ (1,0) [0|0] "" Vector__XXX - SG_ SportButtonState : 26|1@1+ (1,0) [0|1] "" XXX - -BO_ 437 HeatFlow_LoadTorqueClimate: 8 IHKA - -BO_ 1152 NetworkManagment1: 8 XXX - -BO_ 1170 NetworkManagment2: 8 XXX - -BO_ 1175 NetworkManagment3: 8 XXX - -BO_ 1176 NetworkManagment4: 8 XXX - -BO_ 1193 NetworkManagment5: 8 XXX - -BO_ 1246 GWS_ShiftLeverHeartbeat: 8 XXX - SG_ IgnOff : 12|1@0+ (1,0) [0|3] "" XXX - -BO_ 438 HeatFlowEngine: 8 DME - -BO_ 784 AmbientTemperature_RelativeTime: 8 Kombi - -BO_ 821 ElectricFuelPumpStatus: 8 EKP - -BO_ 1007 EngineOBD_data: 8 DME + SG_ ST_INFS : 14|2@1+ (1,0) [0|0] "" XXX + SG_ TORQ_AVL_MIN : 16|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX + SG_ TORQ_AVL_MAX : 28|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX + SG_ TORQ_AVL_SPAR_NEG : 40|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX + SG_ TORQ_AVL_SPAR_POS : 52|12@1- (0.5,0) [-1023.5|1023.5] "Nm" XXX -BO_ 1432 ServicesDKG: 8 XXX +BO_ 256 OpenpilotEngage: 8 XXX + SG_ OpenpilotEngageStatus : 0|8@1+ (1,0) [0|255] "" XXX + SG_ SET_SPEED : 15|8@0+ (1,0) [0|255] "kph" XXX -BO_ 309 CrashDisconnectControl: 8 ACSM +BO_ 170 AccPedal: 8 DME + SG_ Checksum_0xaa : 0|8@1- (1,0) [0|65535] "" XXX + SG_ Counter_0xaa : 8|4@1+ (1,0) [0|15] "" XXX + SG_ AcceleratorPedalPercentage : 16|16@1+ (0.04,0) [0|100] "" XXX + SG_ EngineSpeed : 32|16@1+ (0.25,0) [0|8000] "rpm" XXX + SG_ ThrottlelActive : 50|1@0+ (1,0) [0|1] "" XXX + SG_ AcceleratorPedalPressed : 52|1@0+ (1,0) [0|7] "" XXX + SG_ KickDownPressed : 53|1@0+ (1,0) [0|3] "" XXX + SG_ CruisePedalActive : 54|1@0+ (1,0) [0|1] "" XXX + SG_ CruisePedalInactive : 55|1@0+ (1,0) [0|1] "" XXX -BO_ 502 TurnSignals: 2 FRMFA - SG_ TurnSignalIdle : 9|1@0+ (1,0) [0|1] "" XXX - SG_ TurnSignalActive : 8|1@0+ (1,0) [0|1] "" XXX - SG_ RightTurn : 5|1@0+ (1,0) [0|1] "" XXX - SG_ LeftTurn : 4|1@1+ (1,0) [0|1] "" XXX - SG_ HoldActivated : 0|1@1+ (1,0) [0|1] "" XXX +BO_ 172 WheelTorqueDrivetrain2: 8 DME -BO_ 514 Dimming: 8 FRMFA +BO_ 173 Delay_request_ACC: 8 LDM -BO_ 538 LampStatus: 8 FRMFA +BO_ 177 Torque_request_steering: 8 DSC -BO_ 550 RainSensorWiperSpeed: 8 FZD +BO_ 179 Control_steering_assist: 8 AFS -BO_ 578 ClimateFrontStatus: 8 IHKA +BO_ 180 WheelTorqueDriveTrain1: 8 DME -BO_ 704 LCD_lighting: 8 Kombi +BO_ 181 Torque_request_EGS: 8 EGS + SG_ Checksum_Torque_request_EGS : 0|8@1+ (1,0) [0|0] "" XXX + SG_ Gearbox_temperature : 56|8@1+ (1,0) [0|0] "C" XXX -BO_ 758 LightControl: 8 FRMFA +BO_ 182 DynamicCruiseControlTorqueDemand: 5 DSC + SG_ Checksum_0xb6 : 0|8@1+ (1,0) [0|255] "" XXX + SG_ Counter_0xb6 : 8|4@1+ (1,0) [0|14] "" XXX + SG_ TORQ_TAR_DSC : 12|12@1- (0.5,0) [0|1000] "" XXX -BO_ 760 Time_Date: 8 Kombi +BO_ 183 Torque_request_ACC: 8 LDM -BO_ 762 OccupancySeatBeltContact: 8 ACSM - SG_ NEW_SIGNAL_1 : 8|8@1+ (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_2 : 0|8@1+ (1,0) [0|15] "" XXX +BO_ 184 Torque_request_DCT: 8 DKG + SG_ Checksum_0xb8 : 0|8@1+ (1,0) [0|255] "" XXX + SG_ Counter_0xb8 : 8|4@1+ (1,0) [0|15] "" XXX -BO_ 764 TrunkStatus: 8 CAS +BO_ 185 Torque_request_AFS: 8 AFS -BO_ 797 TirePressureStatus: 8 DSC +BO_ 186 TransmissionData: 8 EGS + SG_ GearTar : 0|4@1+ (1,-4) [0|0] "" XXX + SG_ Shifting : 4|1@1+ (1,0) [0|1] "" XXX + SG_ GearRatio : 8|8@1+ (0.05,0) [0|255] "" XXX + SG_ OutputShaftSpeed : 24|16@1- (0.125,0) [0|0] "rpm" XXX + SG_ Checksum_0xba : 40|8@1+ (1,0) [0|255] "" XXX + SG_ Counter_0xba : 48|4@1+ (1,0) [0|14] "" XXX -BO_ 816 Range_Mileage: 8 Kombi +BO_ 187 Target_torque_request: 8 DSC -BO_ 823 StatusFuelControl: 8 DME +BO_ 188 Status_target_torque_conversion: 8 VGSG -BO_ 897 EngineOilLevel: 8 DME +BO_ 189 Torque_request_SSG: 8 SMG -BO_ 940 RunOnTimeTerminal30: 8 JBBF +BO_ 190 Alive_Counter: 8 ARS -BO_ 947 PowerManagmentConsumptionControl: 8 DME +BO_ 191 RequestedWheelTorqueDriveTrain: 5 LDM + SG_ Checksum_0xbf : 0|8@1+ (1,0) [0|255] "" DME + SG_ Counter_0xbf : 8|4@1- (1,0) [0|15] "" DME + SG_ TorqueReq : 16|12@1- (0.5,350) [-1024|1023.96875] "" DME -BO_ 948 PowerBatteryVoltage: 8 DME - SG_ BatteryVoltage : 7|24@0+ (0.001,0) [0|65535] "" XXX +BO_ 192 Alive_Central_Gateway: 8 JBBF -BO_ 958 PowerRunningTime: 8 CAS +BO_ 193 Alive_counter_telephone: 8 CCC -BO_ 1408 ServicesKGM: 8 XXX +BO_ 196 SteeringWheelAngle: 7 DSC + SG_ SteeringPosition : 0|16@1- (0.04395,0) [-600|600] "deg" XXX + SG_ SteeringSpeed : 24|16@1- (0.04395,0) [0|0] "deg/s" XXX -BO_ 1426 ServicesDME: 8 XXX +BO_ 200 SteeringWheelAngle_slow: 6 SZL + SG_ SteeringPosition : 0|16@1- (0.04395,0) [-600|600] "deg" XXX + SG_ Counter_0xc8 : 20|4@1+ (1,0) [0|15] "" XXX + SG_ SteeringSpeed : 24|16@1- (0.04395,0) [-65535|65535] "deg/s" XXX -BO_ 1449 ServicesDSC: 8 XXX +BO_ 201 SteeringWheelAngle_DSC: 8 SZL + SG_ SteeringPosition : 0|16@1- (0.04395,0) [-600|600] "deg" DSC + SG_ Counter_0xc9 : 20|4@1+ (1,0) [0|15] "" DSC + SG_ SteeringPositionComplementLow : 24|11@1- (1,0) [0|1] "" DSC -BO_ 1504 ServicesKOMBI: 8 XXX +BO_ 205 Accelerometer2: 8 XXX + SG_ YawRate : 0|16@1- (1,0) [0|0] "" XXX + SG_ Checksum_0xcd : 16|16@1- (1,0) [0|65535] "" XXX + SG_ LateralAcceleration : 32|16@1- (1,0) [0|0] "" XXX + SG_ Counter_0xcd : 52|4@1+ (1,0) [0|15] "" XXX -BO_ 1522 ServicesKBM: 8 XXX +BO_ 206 WheelSpeeds: 8 DSC + SG_ Wheel_FL : 0|16@1- (0.0625,0) [0|0] "kph" XXX + SG_ Wheel_FR : 16|16@1- (0.0625,0) [0|0] "kph" XXX + SG_ Wheel_RL : 32|16@1- (0.0625,0) [0|0] "kph" XXX + SG_ Wheel_RR : 48|16@1- (0.0625,0) [0|0] "kph" XXX BO_ 209 Accelerometer1: 8 XXX - SG_ Counter_209 : 52|4@1+ (1,0) [0|255] "" XXX + SG_ YawRate : 0|16@1- (1,0) [0|0] "" XXX SG_ Unknown : 16|16@1- (1,0) [0|65535] "" XXX - SG_ YawRate : 0|16@1- (1,0) [0|7] "" XXX SG_ PitchRate : 32|16@1- (1,0) [0|65535] "" XXX - SG_ CRC8_209 : 56|8@1+ (1,0) [0|255] "" XXX - -BO_ 172 WheelTorqueDrivetrain2: 8 DME - -BO_ 128 SYNC: 5 XXX - SG_ State2 : 24|4@1+ (1,0) [0|15] "" XXX - SG_ State1 : 16|8@1+ (1,0) [0|255] "" XXX - SG_ Counter_128 : 28|4@1+ (1,0) [0|15] "" XXX - -BO_ 320 Unknown140: 2 XXX - SG_ State : 7|8@0+ (1,0) [0|255] "" XXX + SG_ Counter_0xd1 : 52|4@1+ (1,0) [0|15] "" XXX + SG_ Checksum_0xd1 : 56|8@1+ (1,0) [0|255] "" XXX BO_ 212 Unknown_d4: 8 XXX SG_ State1 : 40|8@1+ (1,0) [0|255] "" XXX - SG_ Counter_212 : 52|4@1+ (1,0) [0|255] "" XXX - SG_ Checksum_212 : 56|8@1+ (1,0) [0|255] "" XXX + SG_ Counter_0xd4 : 52|4@1+ (1,0) [0|15] "" XXX + SG_ Checksum_0xd4 : 56|8@1+ (1,0) [0|255] "" XXX -BO_ 205 Accelerometer2: 8 XXX - SG_ Counter_205 : 52|4@1+ (1,0) [0|255] "" XXX - SG_ LateralAcceleration : 32|16@1- (1,0) [0|255] "" XXX - SG_ YawRate : 0|16@1- (1,0) [0|255] "" XXX - SG_ CRC8_205 : 16|16@1- (1,0) [0|65535] "" XXX - -BO_ 790 OperationPushButtonDTC: 2 JBBF - SG_ setMe_0x3FFF : 2|14@1+ (1,0) [0|63] "" DSC - SG_ DTC_pressed : 0|1@1+ (1,0) [0|3] "" DSC - -BO_ 1577 Unknown_629: 8 XXX - -BO_ 133 Synchronization_SC_VDA: 8 DSC - -BO_ 173 Delay_request_ACC: 8 LDM - -BO_ 177 Torque_request_steering: 8 DSC - -BO_ 181 Torque_request_EGS: 8 EGS - SG_ Checksum_Torque_request_EGS : 0|8@1+ (1,0) [0|0] "" XXX - SG_ Gearbox_temperature : 56|8@1+ (1,0) [0|0] "C" XXX - -BO_ 183 Torque_request_ACC: 8 LDM - -BO_ 187 Target_torque_request: 8 DSC - -BO_ 188 Status_target_torque_conversion: 8 VGSG - -BO_ 190 Alive_Counter: 8 ARS - -BO_ 192 Alive_Central_Gateway: 8 JBBF - -BO_ 193 Alive_counter_telephone: 8 CCC - -BO_ 213 Request_wheel_torque_brake: 8 DSC +BO_ 213 Request_wheel_torque_brake: 8 LDM + SG_ Checksum_0xd5 : 0|8@1+ (1,0) [0|255] "" DSC + SG_ Counter_0xd5 : 8|4@1- (1,0) [0|15] "" DSC BO_ 215 Alive_Counter_Security: 8 ACSM @@ -416,13 +221,38 @@ BO_ 298 Sensor_data_ROSE: 8 ASCM BO_ 300 input_data_ROSE: 8 DSC +BO_ 304 TerminalStatus: 5 CAS + SG_ ST_KL_R : 1|2@0+ (1,0) [0|3] "" XXX + SG_ ST_KL_15 : 3|2@0+ (1,0) [0|1] "" XXX + SG_ ST_KL_50 : 5|2@0+ (1,0) [0|3] "" XXX + SG_ ST_KEY_VLD : 7|2@0+ (1,0) [0|3] "" XXX + SG_ NO_KEY : 11|4@0+ (1,0) [0|15] "" XXX + SG_ IgnitionOff : 22|1@1+ (1,0) [0|3] "" XXX + SG_ AccOn : 23|1@1+ (1,0) [0|255] "" XXX + SG_ ST_KL15_HW : 31|2@0+ (1,0) [0|3] "" XXX + SG_ Counter_0x130 : 32|4@1+ (1,0) [0|15] "" XXX + SG_ Checksum_0x130 : 36|4@1+ (1,0) [0|255] "" XXX + +BO_ 309 CrashDisconnectControl: 8 ACSM + +BO_ 320 Unknown140: 2 XXX + SG_ State : 7|8@0+ (1,0) [0|255] "" XXX + BO_ 336 Request_1_ACC: 8 LDM + SG_ Checksum_0x150 : 0|8@1+ (1,0) [0|255] "" ACC + SG_ Counter_0x150 : 8|4@1- (1,0) [0|15] "" ACC BO_ 339 Request_2_ACC: 8 LDM + SG_ Checksum_0x153 : 0|8@1+ (1,0) [0|255] "" ACC + SG_ Counter_0x153 : 8|4@1- (1,0) [0|15] "" ACC -BO_ 345 Object_data_ACC: 8 ACC_Sensor +BO_ 345 Object_data_ACC: 8 ACC + SG_ Checksum_0x159 : 0|8@1+ (1,0) [0|255] "" LDM + SG_ Counter_0x159 : 8|4@1- (1,0) [0|15] "" LDM -BO_ 348 Status_ACC: 8 ACC_Sensor +BO_ 348 Status_ACC: 8 ACC + SG_ Checksum_0x15c : 0|8@1+ (1,0) [0|255] "" LDM + SG_ Counter_0x15c : 8|4@1- (1,0) [0|15] "" LDM BO_ 351 Requirement_angle_FFP: 8 LDM @@ -430,20 +260,93 @@ BO_ 357 CLU_Status_VDA: 8 VDA BO_ 370 Acknowledgment_request_Kombination: 8 CCC -BO_ 373 Display_motor_data: 8 DME +BO_ 373 Display_motor_data: 3 DME + SG_ Counter_0x175 : 3|4@0+ (1,0) [0|15] "" XXX BO_ 400 display_ACC: 8 LDM +BO_ 402 GearSelectorSwitch_1: 4 XXX + SG_ Counter_0x192 : 24|4@1+ (1,0) [0|14] "" XXX + +BO_ 403 DynamicCruiseControlStatus: 8 DSC + SG_ Checksum_0x193 : 0|4@1+ (1,0) [0|15] "" XXX + SG_ Counter_0x193 : 7|4@0+ (1,0) [0|15] "" XXX + SG_ CruiseControlSetpointSpeed : 15|8@0+ (1,-2) [0|255] "kph/mph" XXX + SG_ CruiseActive : 43|1@0+ (1,0) [0|1] "" XXX + SG_ CruiseSpeedChangeRequest : 48|1@0+ (1,0) [0|1] "" XXX + +BO_ 404 CruiseControlStalk: 4 SZL + SG_ Checksum_0x194 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ Counter_0x194 : 11|4@0+ (1,0) [0|14] "" XXX + SG_ requests_0xF : 15|4@0+ (1,0) [0|15] "" XXX + SG_ plus1 : 16|1@0+ (1,0) [0|1] "" XXX + SG_ plus5 : 17|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ minus1 : 18|1@0+ (1,0) [0|1] "" XXX + SG_ minus5 : 19|1@0+ (1,0) [0|1] "" Vector__XXX + SG_ cancel : 20|1@0+ (1,0) [0|1] "" XXX + SG_ resume : 22|1@0+ (1,0) [0|1] "" XXX + SG_ cancel_lever_up : 23|1@0+ (1,0) [0|1] "" XXX + SG_ setMe_0xFC : 31|8@0+ (1,0) [255|255] "" XXX + BO_ 405 Operation_push_button_MSA: 8 IHKA +BO_ 408 GearSelectorSwitch: 5 GWS + SG_ Counter_0x198 : 0|4@1+ (1,0) [0|14] "" XXX + SG_ m3ShifterPosition : 4|4@1+ (1,0) [0|15] "" XXX + SG_ m3ShifterPositionXOR1111 : 8|4@1+ (1,0) [0|15] "" XXX + SG_ ShifterPosition : 12|4@1+ (1,0) [0|0] "" Vector__XXX + SG_ ShifterPositionXOR1111 : 16|4@1+ (1,0) [0|15] "" XXX + SG_ param1 : 20|2@1+ (1,0) [0|3] "" XXX + SG_ param1XOR11 : 22|2@1+ (1,0) [0|3] "" XXX + SG_ ParkButtonFirst : 24|2@1+ (1,0) [0|3] "" XXX + SG_ ParkButtonSecond : 26|2@1+ (1,0) [0|3] "" XXX + SG_ SideButton : 28|2@1+ (1,0) [0|3] "" XXX + SG_ SideButtonXOR11 : 30|2@1+ (1,0) [0|3] "" XXX + SG_ param5 : 32|2@1+ (1,0) [0|3] "" XXX + SG_ SportButtonPressed : 34|2@1+ (1,0) [0|255] "" XXX + SG_ M3_button : 36|2@1+ (1,0) [0|3] "" XXX + SG_ always11 : 38|2@1+ (1,0) [0|3] "" XXX + +BO_ 414 StatusDSC_KCAN: 8 DSC + SG_ DSC_full_off : 8|4@1+ (1,0) [0|1] "" XXX + SG_ DTC_on : 12|1@1+ (1,0) [0|1] "" XXX + SG_ Counter_0x19e : 20|4@1+ (1,0) [0|15] "" XXX + SG_ BrakeStates : 40|8@1+ (1,0) [0|255] "" XXX + SG_ BrakePressure : 48|8@1- (1,0) [0|0] "" XXX + SG_ Checksum_0x19e : 56|8@1+ (1,0) [0|255] "" XXX + +BO_ 416 Speed: 8 DSC + SG_ VehicleSpeed : 0|12@1- (0.103,0) [0|0] "kph" XXX + SG_ MovingForward : 12|1@1+ (1,0) [0|1] "" XXX + SG_ MovingReverse : 13|1@1+ (1,0) [0|1] "" XXX + SG_ LongAcc : 16|12@1- (0.025,0) [0|0] "m/s^2" XXX + SG_ LatlAcc : 28|12@1- (0.025,0) [0|0] "m/s^2" XXX + SG_ YawRate : 40|12@1- (0.05,0) [0|0] "deg/s" XXX + SG_ Counter_0x1a0 : 52|4@1+ (1,0) [0|14] "" XXX + SG_ Checksum_0x1a0 : 56|8@1+ (1,0) [0|255] "" XXX + +BO_ 418 TransimissionData2: 8 EGS + SG_ Counter_0x1a2 : 28|4@1+ (1,0) [0|14] "" XXX + SG_ ManualMode : 50|1@0+ (1,0) [0|1] "" XXX + SG_ Checksum_0x1a2 : 56|8@1+ (1,0) [0|255] "" XXX + BO_ 419 Raw_data_longitudinal_acceleration: 8 DSC +BO_ 422 DistanceRoute: 8 DSC + BO_ 423 actuation_request_EMF: 8 DSC BO_ 426 Effect_ErgoCommander: 8 CCC BO_ 428 Status_ARS_module: 8 ARS +BO_ 436 InstrumentClusterStatus_KOMBI: 8 CCC + SG_ HandbrakeActive : 41|1@1+ (1,0) [0|1] "" XXX + +BO_ 437 HeatFlow_LoadTorqueClimate: 7 IHKA + +BO_ 438 HeatFlowEngine: 7 DME + BO_ 440 Operation_ErgoCommander: 8 ZBE BO_ 450 Distance_message_PDC: 8 PDC @@ -452,6 +355,34 @@ BO_ 451 Distance_message_2_PDC: 8 PDC BO_ 454 Acoustic_message_PDC: 8 PDC +BO_ 464 EngineData: 8 DME + SG_ TEMP_ENG : 0|8@1+ (1,-48) [0|0] "C" XXX + SG_ TEMP_EOI : 8|8@1+ (1,-48) [0|0] "C" XXX + SG_ Counter_0x1d0 : 16|4@1+ (1,0) [0|14] "" XXX + SG_ ST_ENG_RUN : 20|2@1+ (1,0) [0|0] "" XXX + SG_ ST_SW_WAUP : 22|2@1+ (1,0) [0|0] "" XXX + SG_ AIP_ENG : 24|8@1+ (2,598) [600|1106] "hPa" XXX + SG_ IJV_FU : 32|16@1+ (1,-48) [0|0] "C" XXX + SG_ CTR_SLCK : 48|2@1+ (1,0) [0|0] "" XXX + SG_ RPM_IDLG_TAR : 56|8@1+ (5,0) [0|1270] "1/min" XXX + +BO_ 466 TransmissionDataDisplay: 6 EGS + SG_ ShiftLeverPosition : 0|4@1+ (1,0) [0|8] "" XXX + SG_ ShiftLeverPositionXOR : 4|4@1+ (1,0) [0|0] "" Vector__XXX + SG_ GearAct : 12|4@1+ (1,-4) [0|15] "" XXX + SG_ SportButtonState : 26|1@1+ (1,0) [0|1] "" XXX + SG_ Counter_0x1d2 : 28|4@1+ (1,0) [0|14] "" XXX + SG_ ShiftLeverMode : 32|2@1+ (1,0) [0|3] "" XXX + SG_ xFF : 40|8@1+ (1,0) [0|255] "" XXX + +BO_ 470 SteeringButtons: 2 SZL + SG_ Telephone : 0|1@0+ (1,0) [0|1] "" XXX + SG_ Volume_DOWN : 2|1@0+ (1,0) [0|1] "" XXX + SG_ Volume_UP : 3|1@0+ (1,0) [0|1] "" XXX + SG_ Previous_down : 4|1@0+ (1,0) [0|1] "" XXX + SG_ Next_up : 5|1@0+ (1,0) [0|1] "" XXX + SG_ VoiceControl : 8|1@0+ (1,0) [0|1] "" XXX + BO_ 472 Operation_air_conditioning_air_distribution_FA: 8 CCC BO_ 473 Operation_pushbutton_MDrive: 8 SZL @@ -478,9 +409,22 @@ BO_ 491 Operation_active_seat_FA: 8 IHKA BO_ 492 Operation_active_seat_BF: 8 IHKA +BO_ 493 Operation_backrest_adjust_FA: 8 SZM + BO_ 494 Steering_column_switch_operation: 8 FRMFA -BO_ 499 Operation_seat_memory_FA: 8 XXX +BO_ 495 Operation_backrest_adjust_BF: 8 SZM + +BO_ 498 Operation_seat_memory_BF: 8 SZM + +BO_ 499 Operation_seat_memory_FA: 8 SZM + +BO_ 502 TurnSignals: 2 FRMFA + SG_ HoldActivated : 0|1@1+ (1,0) [0|1] "" XXX + SG_ LeftTurn : 4|1@1+ (1,0) [0|1] "" XXX + SG_ RightTurn : 5|1@0+ (1,0) [0|1] "" XXX + SG_ TurnSignalActive : 8|1@0+ (1,0) [0|1] "" XXX + SG_ TurnSignalIdle : 9|1@0+ (1,0) [0|1] "" XXX BO_ 504 Operation_SHD_MDS: 8 FZD @@ -492,8 +436,15 @@ BO_ 509 Status_request_EMF_KCAN: 8 EMF BO_ 510 Crash: 8 ACSM +BO_ 512 CruiseControlStatus: 8 DME + SG_ CruiseControlSetpointSpeed : 7|8@0+ (0.25,0) [0|0] "mph" XXX + SG_ CruiseControlInactiveFlag : 12|1@0+ (1,0) [0|1] "" XXX + SG_ CruiseCoontrolActiveFlag : 13|1@0+ (1,0) [0|1] "" XXX + BO_ 513 Status_EMF_KCAN: 8 EMF +BO_ 514 Dimming: 2 FRMFA + BO_ 517 Acoustic_request_Kombi: 8 Kombi BO_ 518 Control_Display_Shiftlights: 8 DME @@ -510,20 +461,22 @@ BO_ 529 Status_HUD: 8 HUD BO_ 530 Height_levels_air_spring: 8 EHC +BO_ 538 LampStatus: 8 FRMFA + BO_ 540 Operation_NightVision: 8 CCC BO_ 542 Status_NightVision: 8 NVC BO_ 548 Operation_push_button_NSW: 8 FRMFA +BO_ 550 RainSensorWiperSpeed: 5 FZD + BO_ 552 Operation_special_function: 8 CCC BO_ 554 Status_BFS: 8 JBBF BO_ 556 Operation_push_button_NSL: 8 FRMFA -BO_ 558 Status_BFSH: 8 XXX - BO_ 562 Status_FAS: 8 JBBF BO_ 566 Status_FASH: 8 XXX @@ -536,6 +489,8 @@ BO_ 573 Request_display_climate: 8 IHKA BO_ 574 Status_Klima_Fond: 8 FKA +BO_ 578 ClimateFrontStatus: 8 IHKA + BO_ 582 Status_air_conditioning_front_control_unit: 8 IHKA BO_ 584 Status_rear_view_camera: 8 RFK @@ -554,6 +509,10 @@ BO_ 600 Status_Transmission_Passive_Access: 8 PGS BO_ 604 Operation_of_climate_additional_programs: 8 CCC +BO_ 619 Operation_blinds_BF: 8 KGM + +BO_ 620 Operation_blinds_FA: 8 KGM + BO_ 621 Operation_blinds_MK: 8 IHKA BO_ 622 Control_FH_SHD_central_comfort: 8 CAS @@ -590,6 +549,16 @@ BO_ 674 Operation_of_climate_stand_functions: 8 CCC BO_ 676 Operation_personalization: 8 Kombi +BO_ 678 WiperSwitch: 2 SZL + SG_ AutoWipersOn : 0|1@1+ (1,0) [0|3] "" XXX + +BO_ 690 WheelPressure_KCAN: 8 DSC + +BO_ 691 AccelerationData: 5 DSC + SG_ Checksum_0x2b3 : 7|8@0+ (1,0) [0|15] "" XXX + SG_ Counter_0x2b3 : 8|4@1+ (1,0) [0|15] "" XXX + SG_ Deceleration : 12|8@1- (1,0) [0|255] "" XXX + BO_ 692 DWA_Alarm: 8 DWA BO_ 694 Control_horn_DWA: 8 DWA @@ -606,6 +575,8 @@ BO_ 702 Switch_status_display: 8 VSW BO_ 703 Water_valve_control: 8 IHKA +BO_ 704 LCD_lighting: 3 Kombi + BO_ 706 Temperatur_Ist_Fond: 8 FKA BO_ 711 Display_Kombination_extended: 8 DME @@ -648,14 +619,24 @@ BO_ 750 Status_climate_additional_programs: 8 IHKA BO_ 752 Status_air_condition_functions: 8 IHKA -BO_ 753 Status_driver_detection: 8 MRSZ +BO_ 753 Status_driver_detection: 3 MRSZ BO_ 755 Display_switching_instruction: 8 DME BO_ 756 Air_conditioning_control_SH_ZH_auxiliary_water_pump: 8 IHKA +BO_ 758 LightControl: 2 FRMFA + BO_ 759 Units: 8 Kombi +BO_ 760 Time_Date: 8 Kombi + +BO_ 762 OccupancySeatBeltContact: 5 ACSM + SG_ NEW_SIGNAL_2 : 0|8@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_1 : 8|8@1+ (1,0) [0|255] "" XXX + +BO_ 764 TrunkStatus: 8 CAS + BO_ 768 Status_RSE: 8 RSE BO_ 772 Status_Gang: 8 EGS @@ -668,6 +649,8 @@ BO_ 775 Operation_button_flap_convertible_top: 8 IHKA BO_ 776 Status_MSA: 8 DME +BO_ 784 AmbientTemperature_RelativeTime: 8 Kombi + BO_ 785 Refill_quantity: 8 Kombi BO_ 786 Service_Call_Teleservice: 8 Kombi @@ -678,6 +661,10 @@ BO_ 788 Status_driving_light: 8 FZD BO_ 789 Vehicle_mode: 8 JBBF +BO_ 790 OperationPushButtonDTC: 2 JBBF + SG_ DTC_pressed : 0|1@1+ (1,0) [0|3] "" DSC + SG_ setMe_0x3FFF : 2|14@1+ (1,0) [0|63] "" DSC + BO_ 791 Operation_button_parking_aids: 8 IHKA BO_ 792 Status_antennas_Passive_Access: 8 PGS @@ -690,6 +677,8 @@ BO_ 795 Operation_of_tailgate_interior_button: 8 IHKA BO_ 796 Status_tire_pressure: 8 RDC +BO_ 797 TirePressureStatus: 2 DSC + BO_ 801 Operation_button_camera_BF: 8 IHKA BO_ 802 Damper_current: 8 EDCK @@ -698,16 +687,24 @@ BO_ 806 Status_damper_program: 8 VDM BO_ 808 Relativzeit: 8 Kombi +BO_ 810 Control_ALC: 8 XXX + BO_ 813 Display_HDC: 8 DSC BO_ 814 Status_climate_internal_control_info: 8 IHKA +BO_ 816 Range_Mileage: 8 Kombi + BO_ 817 Programming_of_step_cruise_control: 8 CCC -BO_ 818 Driver_display_speed_range: 8 DME +BO_ 818 Driver_display_speed_range: 2 DME + +BO_ 821 ElectricFuelPumpStatus: 8 EKP BO_ 822 Display_check_control_message_role: 8 Kombi +BO_ 823 StatusFuelControl: 2 DME + BO_ 824 Control_display_check_control_message: 8 Kombi BO_ 825 Status_display_climate: 8 CCC @@ -720,12 +717,15 @@ BO_ 830 Status_Monitor_Fond_2: 8 CID BO_ 841 Raw_data_level_tank: 8 JBBF -BO_ 843 Seat_back_lock_status_FA: 8 SM_FA +BO_ 843 Status_seat_back_lock_FA: 8 SM_FA BO_ 845 Status_seat_back_lock_BF: 8 SM_BF -BO_ 847 Status_contact_handbrake: 8 JBBF - SG_ Handbrake_push : 0|2@1+ (1,0) [0|3] "" XXX +BO_ 846 Navi_sys_info: 8 CCC + +BO_ 847 Status_contact_handbrake: 2 JBBF + SG_ Handbrake__pushed_down : 0|1@0+ (1,0) [0|1] "" XXX + SG_ Handbrake_pulled_up : 1|1@1+ (1,0) [0|3] "" XXX BO_ 858 Appointment_Condition_Based_Service: 8 CCC @@ -747,9 +747,11 @@ BO_ 871 Control_display_of_demand_oriented_service: 8 Kombi BO_ 877 Setting_the_driving_dynamics_switch: 8 JBBF +BO_ 884 WheelToleranceAdjustment: 5 DSC + BO_ 886 Status_wear_lamella: 8 VGSG -BO_ 893 Status_DKG: 8 DKG +BO_ 893 Status_DKG: 2 DKG BO_ 894 Temperatur_Bremse: 8 DSC @@ -757,9 +759,11 @@ BO_ 895 Range_of_diesel_exhaust_gas_additive: 8 DDE1 BO_ 896 chassis_number: 8 CAS +BO_ 897 EngineOilLevel: 2 DME + BO_ 898 Electronic_engine_oil_dipstick_M: 8 DME -BO_ 899 Motor_data_2: 8 DME +BO_ 899 Motor_data_2: 4 DME BO_ 904 Vehicle_type: 8 CAS @@ -787,8 +791,19 @@ BO_ 937 Status_motor_control_CKM: 8 DME BO_ 939 Status_Shiftlights_CKM: 8 Kombi +BO_ 940 RunOnTimeTerminal30: 2 JBBF + BO_ 944 Status_gear_reverse: 8 FRMFA +BO_ 945 TransmissionData3: 6 DKG + SG_ Checksum_0x3b1 : 0|8@1+ (1,0) [0|19] "" XXX + SG_ Counter_0x3b1 : 8|4@1+ (1,0) [0|14] "" XXX + +BO_ 947 PowerManagmentConsumptionControl: 6 DME + +BO_ 948 PowerBatteryVoltage: 8 DME + SG_ BatteryVoltage : 7|24@0+ (0.001,0) [0|65535] "" XXX + BO_ 949 Status_water_valve: 8 JBBF BO_ 950 Position_window_lifter_FAT: 8 FRMFA @@ -805,6 +820,8 @@ BO_ 956 Position_window_lifter_security_vehicle: 8 XXX BO_ 957 Status_consumer_shutdown: 8 FRMFA +BO_ 958 PowerRunningTime: 2 CAS + BO_ 959 Position_window_rear_window: 8 CTM BO_ 960 Configuration_FAS: 8 SM_FA @@ -853,61 +870,77 @@ BO_ 995 Status_tailgate_CKM: 8 HKL BO_ 996 Configuration_rear_view_camera_CKM: 8 CCC +BO_ 1007 EngineOBD_data: 3 DME +BO_ 1152 NetworkManagment1: 8 XXX +BO_ 1170 NetworkManagment2: 8 XXX -CM_ SG_ 170 ThrottlelPressed "Active when accelerator pedal pressed or cruise control: drives"; -CM_ SG_ 170 AcceleratorPedalPressed "Active only when driver actually presses the pedal"; -CM_ SG_ 170 AcceleratorPedalPercentage "ToDo Factor to be adjusted"; -CM_ SG_ 404 plus1mph_request "Appears when +1mph/kph stalk is depressed"; -CM_ SG_ 404 minus1mph_request "Appears when -1mph/kph stalk is depressed"; -CM_ SG_ 404 Cancel_request_up_stalk "Appears when cancel stalk (up) is depressed"; -CM_ SG_ 404 Cancel_request_up_or_down_stalk "Appears when cancel stalk (up or down) is depressed"; -CM_ SG_ 404 Resume_request "It appears when resume stalk button is depressed"; -CM_ SG_ 404 Counter_404 "Message is sent at higher rate when cruise stalk is pressed"; +BO_ 1175 NetworkManagment3: 8 XXX -CM_ SG_ 168 BrakePressed "Brake when driver presses the brake or hill hold"; -CM_ SG_ 168 EngineTorque "Engine torque without inertia - combustion torque"; -CM_ SG_ 168 EngineTorqueWoInterv "Engine torque without inertia and without shift intervention"; -CM_ SG_ 168 ALIV_TORQ_1_DME "Counter TORQ_1"; +BO_ 1176 NetworkManagment4: 8 XXX -CM_ SG_ 169 ALIV_TORQ_2_DME "Counter TORQ_2"; +BO_ 1193 NetworkManagment5: 8 XXX -CM_ SG_ 182 TORQ_TAR_DSC "torque target DSC"; +BO_ 1246 GWS_ShiftLeverHeartbeat: 8 XXX + SG_ IgnOff : 12|1@0+ (1,0) [0|3] "" XXX -CM_ SG_ 403 CruiseControlSetpointSpeed "Speed target - unit depends on locale"; -CM_ SG_ 186 GearTar "Values corresponds to forward gears. TBD Add enums for park, reverse"; -CM_ SG_ 466 GearAct "TransmissionDataDisplay"; -CM_ SG_ 414 DSC_full_off "0x4 enabling, 0xA enabled. TBD"; +BO_ 1408 ServicesKGM: 8 XXX + +BO_ 1426 ServicesDME: 8 XXX + +BO_ 1432 ServicesDKG: 8 XXX -CM_ SG_ 416 YawRate "Lateral Acceleration"; +BO_ 1449 ServicesDSC: 8 XXX +BO_ 1504 ServicesKOMBI: 8 XXX + +BO_ 1522 ServicesKBM: 8 XXX + +BO_ 1577 Unknown_629: 8 XXX + +CM_ SG_ 168 Counter_0xa8 "Counter TORQ_1"; +CM_ SG_ 168 EngineTorque "Engine torque without inertia - combustion torque"; +CM_ SG_ 168 EngineTorqueWoInterv "Engine torque without inertia and without shift intervention"; +CM_ SG_ 168 BrakePressed "Brake when driver presses the brake or hill hold"; +CM_ SG_ 170 AcceleratorPedalPercentage "ToDo Factor to be adjusted"; +CM_ SG_ 170 ThrottlelActive "Active in cruise control when engine braking"; +CM_ SG_ 170 AcceleratorPedalPressed "Active only when driver actually presses the pedal"; +CM_ SG_ 182 TORQ_TAR_DSC "torque target DSC"; +CM_ SG_ 186 GearTar "Values corresponds to forward gears. TBD Add enums for park, reverse"; +CM_ SG_ 403 CruiseControlSetpointSpeed "Speed target - unit depends on locale"; +CM_ SG_ 404 Counter_0x194 "Message is sent at higher rate when cruise stalk is pressed"; +CM_ SG_ 404 plus1 "Appears when +1mph/kph stalk is depressed"; +CM_ SG_ 404 minus1 "Appears when -1mph/kph stalk is depressed"; +CM_ SG_ 404 cancel "Appears when cancel stalk (up or down) is depressed"; +CM_ SG_ 404 resume "It appears when resume stalk button is depressed"; +CM_ SG_ 404 cancel_lever_up "Appears when cancel stalk (up) is depressed"; +CM_ SG_ 408 ShifterPosition "0001 = N|R, 0010 = R, 0011 = N|D, 0100 = D, 0101 = -1, 0110 = +1, 0111 = ManualMode, 1000 = inBetween,"; +CM_ SG_ 408 param1 "can not be 0x03. Always 0 in 135i."; +CM_ SG_ 408 ParkButtonFirst "0x1 = pressed"; CM_ SG_ 408 ParkButtonSecond "Redundant buton. 0x1 = pressed;"; CM_ SG_ 408 SideButton "0x1 = pressed;"; -CM_ SG_ 408 SportButtonPressed "0x1 = pressed;"; -CM_ SG_ 408 M3_button "M3 POWER (?)"; CM_ SG_ 408 SideButtonXOR11 "Complement value"; -CM_ SG_ 408 param1 "can not be 0x03. Always 0 in 135i."; CM_ SG_ 408 param5 "Always 0 in 135i."; -CM_ SG_ 408 ParkButtonFirst "0x1 = pressed"; -CM_ SG_ 408 ShifterPosition "0001 = N|R, 0010 = R, 0011 = N|D, 0100 = D, 0101 = -1, 0110 = +1, 0111 = ManualMode, 1000 = inBetween,"; - +CM_ SG_ 408 SportButtonPressed "0x1 = pressed;"; +CM_ SG_ 408 M3_button "M3 POWER (?)"; +CM_ SG_ 414 DSC_full_off "0x4 enabling, 0xA enabled. TBD"; +CM_ SG_ 416 LongAcc "Longitudinal acceleration"; +CM_ SG_ 416 LatlAcc "Lateral acceleration"; +CM_ SG_ 466 GearAct "TransmissionDataDisplay"; +CM_ SG_ 466 SportButtonState "Only selected powertrains. Makes throttle, overboost, suspension, steering, shiftpoints more aggressive"; CM_ SG_ 466 ShiftLeverMode "On the dashboard, there are displayed: D1...D7 in Normal. S1...S6 in Sport. M1..M7 in Manual"; - -CM_ SG_ 502 TurnSignalIdle "Turn signal off"; -CM_ SG_ 502 TurnSignalActive "Turn signal on or transitioning"; -CM_ SG_ 502 RightTurn "Indicates right blinker or when steering returning right clears left blinker"; -CM_ SG_ 502 LeftTurn "Indicates left blinker or when steering returning left clears right blinker"; CM_ SG_ 502 HoldActivated "Spikes down if blinker cleared with timeout or turn. Stays off if blinker cleared with stalk"; - -CM_ SG_ 790 setMe_0x3FFF "All ones"; +CM_ SG_ 502 LeftTurn "Indicates left blinker or when steering returning left clears right blinker"; +CM_ SG_ 502 RightTurn "Indicates right blinker or when steering returning right clears left blinker"; +CM_ SG_ 502 TurnSignalActive "Turn signal on or transitioning"; +CM_ SG_ 502 TurnSignalIdle "Turn signal off"; +CM_ SG_ 691 Deceleration "Filtered deceleration. Opposite to AccY signal"; CM_ SG_ 790 DTC_pressed "Traction control off. Message transmitted when pressed. Few presses may be required followed by off."; - -VAL_ 408 ShifterPosition 1 "D" 2 "S" 3 "N" 4 "R" 5 "P" ; - -VAL_ 464 ST_SW_WAUP 3 "signal invalid" 2 "EGS forced switching active" 1 "engine warm" 0 "warming up" ; -VAL_ 464 ST_ENG_RUN 3 "signal invalid" 2 "engine running" 1 "engine starting" 0 "engine off" ; -VAL_ 464 CTR_SLCK 3 "signal invalid" 2 "requirement Shiftlock" 1 "no requirement Shiftlock" 0 "not allowed" ; - -VAL_ 466 ShiftLeverMode 0 "Normal" 1 "Sport" 2 "Manual" ; -VAL_ 466 ShiftLeverPosition 0 "Off" 1 "P" 2 "R" 4 "N" 8 "D" ; +CM_ SG_ 790 setMe_0x3FFF "All ones"; +VAL_ 408 ShifterPosition 1 "D" 2 "S" 3 "N" 4 "R" 5 "P"; +VAL_ 464 ST_ENG_RUN 3 "signal invalid" 2 "engine running" 1 "engine starting" 0 "engine off"; +VAL_ 464 ST_SW_WAUP 3 "signal invalid" 2 "EGS forced switching active" 1 "engine warm" 0 "warming up"; +VAL_ 464 CTR_SLCK 3 "signal invalid" 2 "requirement Shiftlock" 1 "no requirement Shiftlock" 0 "not allowed"; +VAL_ 466 ShiftLeverPosition 0 "Off" 1 "P" 2 "R" 4 "N" 8 "D"; +VAL_ 466 ShiftLeverMode 0 "Normal" 1 "Sport" 2 "Manual"; \ No newline at end of file diff --git a/opendbc_repo/opendbc/dbc/ocelot_controls.dbc b/opendbc_repo/opendbc/dbc/ocelot_controls.dbc new file mode 100644 index 0000000000..aec1abfde9 --- /dev/null +++ b/opendbc_repo/opendbc/dbc/ocelot_controls.dbc @@ -0,0 +1,113 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: EON PED IBST SSC +VAL_TABLE_ BRAKEMODE 3 "Reserved" 2 "Position Control" 1 "Relative Control" 0 "Disengadged" ; +VAL_TABLE_ STATUS 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +VAL_TABLE_ YESNO 1 "Yes" 0 "No" ; + +BO_ 512 PEDAL_GAS_COMMAND: 6 EON + SG_ GAS_COMMAND : 8|16@1+ (1,0) [0|1] "" PED + SG_ GAS_COMMAND2 : 24|16@1+ (1,0) [0|1] "" PED + SG_ ENABLE : 47|1@1+ (1,0) [0|1] "" PED + SG_ COUNTER : 40|4@1+ (1,0) [0|15] "" PED + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" PED + +BO_ 513 PEDAL_GAS_SENSOR: 6 PED + SG_ PED_GAS : 8|16@1+ (1,0) [0|1] "" EON + SG_ PED_GAS2 : 24|16@1+ (1,0) [0|1] "" EON + SG_ STATE : 44|4@1+ (1,0) [0|15] "" EON + SG_ COUNTER : 40|4@1+ (1,0) [0|15] "" EON + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" EON + +BO_ 526 OCELOT_BRAKE_COMMAND: 6 EON + SG_ BRAKE_POSITION_COMMAND : 32|12@1+ (0.015625,-5) [-5|47] "mm" IBST + SG_ BRAKE_RELATIVE_COMMAND : 16|16@1+ (0.0078125,-252) [-252|252] "" IBST + SG_ BRAKE_MODE : 12|2@1+ (1,0) [0|3] "" IBST + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" IBST + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" IBST + +BO_ 527 OCELOT_BRAKE_STATUS: 5 IBST + SG_ BRAKE_PEDAL_POSITION : 20|12@1+ (0.015625,-5) [-5|47] "mm" EON + SG_ BRAKE_APPLIED : 18|1@1+ (1,0) [0|1] "" EON + SG_ DRIVER_BRAKE_APPLIED : 17|1@1+ (1,0) [0|1] "" EON + SG_ BRAKE_OK : 16|1@1+ (1,0) [0|1] "" EON + SG_ STATUS : 12|4@1+ (1,0) [0|15] "" EON + SG_ EXT_STATUS1 : 32|4@1+ (1,0) [0|15] "" EON + SG_ EXT_STATUS2 : 36|4@1+ (1,0) [0|15] "" EON + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" EON + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" EON + +BO_ 558 STEERING_COMMAND: 5 EON + SG_ STEER_TORQUE : 32|8@1- (0.125,0) [-16|15.875] "Nm" SSC + SG_ STEER_ANGLE : 16|16@1- (0.125,0) [-4096|4095.875] "deg" SSC + SG_ STEER_MODE : 12|2@1+ (1,0) [0|3] "" SSC + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" SSC + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" SSC + +BO_ 559 STEERING_STATUS: 8 SSC + SG_ STEERING_ANGLE : 40|16@1- (0.125,0) [-4096|4095.875] "deg" EON + SG_ STEERING_SPEED : 24|8@1- (0.015625,0) [-2|1.984375] "rev/s" EON + SG_ STEERING_TORQUE : 16|8@1- (0.125,0) [-16|15.875] "Nm" EON + SG_ CONTROL_STATUS : 12|4@1+ (1,0) [0|15] "" EON + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" EON + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" EON + SG_ TEMPERATURE : 32|8@1+ (1,-60) [-60|195] "C" EON + SG_ DEBUG_STATES : 56|8@1+ (1,0) [0|255] "" EON + + + +CM_ SG_ 558 STEER_TORQUE "Steering torque request"; +CM_ SG_ 558 STEER_ANGLE "Steering relative angle request for internal close loop controller"; +CM_ SG_ 558 STEER_MODE "Control mode request. +Off - instant 0 torque +TorqueControl - use steer_torque, +AngleControl - use steer_angle as relative target and steer_torque as feedforward, +SoftOff - ramp torque down with constant rate, command Off after SoftOff to resume control"; +CM_ SG_ 558 COUNTER "Rolling counter"; +CM_ SG_ 558 CHECKSUM "8bit sum of all bytes and message id"; +CM_ SG_ 559 STEERING_ANGLE "Steering angle calculated from motor position sensor"; +CM_ SG_ 559 STEERING_SPEED "Steering speed calculated from motor position sensor"; +CM_ SG_ 559 STEERING_TORQUE "Motor torque calculated at the steering"; +CM_ SG_ 559 CONTROL_STATUS "Bitwise states: bit0 - sensored torque control is enabled, bit1 - sensor feedback enabled, bit2 - openloop soft off active, bit3 - internal closeloop mode enabled"; +CM_ SG_ 559 COUNTER "Rolling counter"; +CM_ SG_ 559 CHECKSUM "8bit sum of all bytes and message id"; +CM_ SG_ 559 TEMPERATURE "Motor PCB temperature"; +CM_ SG_ 559 DEBUG_STATES "Bitwise status. Refer to source"; +VAL_ 526 BRAKE_MODE 3 "Reserved" 2 "Position Control" 1 "Relative Control" 0 "Disengadged" ; +VAL_ 527 BRAKE_OK 1 "Yes" 0 "No" ; +VAL_ 527 STATUS 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; +VAL_ 558 STEER_MODE 0 "Off" 1 "TorqueControl" 2 "AngleControl" 3 "SoftOff" ; + diff --git a/panda/board/main.c b/panda/board/main.c index 9770f8368f..731adbfa47 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -253,9 +253,10 @@ static void tick_handler(void) { // clear heartbeat engaged state heartbeat_engaged = false; - if (current_safety_mode != SAFETY_SILENT) { - set_safety_mode(SAFETY_SILENT, 0U); - } + // BMW: Keep panda always active, never go to SILENT mode + // if (current_safety_mode != SAFETY_SILENT) { + // set_safety_mode(SAFETY_SILENT, 0U); + // } if (power_save_status != POWER_SAVE_STATUS_ENABLED) { set_power_save_state(POWER_SAVE_STATUS_ENABLED); @@ -336,8 +337,8 @@ int main(void) { fan_init(); } - // init to SILENT and can silent - set_safety_mode(SAFETY_SILENT, 0U); + // init to NOOUTPUT (allows CAN output, doesn't fall back to SILENT) + set_safety_mode(SAFETY_NOOUTPUT, 0U); // enable CAN TXs enable_can_transceivers(true); diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3dd2a96c8a..d4c4283e77 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -197,7 +197,7 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - + lateral_jerk_setpoint = 0 lateral_jerk_measurement = 0 lookahead_lateral_jerk = 0 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index db50f7b940..83f20568d8 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -286,7 +286,8 @@ def update_events(self, CS): safety_mismatch = False #carrot # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda - if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: + # BMW: Disable safety mismatch check entirely + if False and ((safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200): self.events.add(EventName.controlsMismatch) if log.PandaState.FaultType.relayMalfunction in pandaState.faults: diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index 0468adefbe..bcf86247c7 100644 --- a/selfdrive/ui/carrot.cc +++ b/selfdrive/ui/carrot.cc @@ -224,8 +224,8 @@ static inline void fill_rect(NVGcontext* vg, const Rect1& r, const NVGcolor* col if (stroke_width > 0) { nvgStrokeWidth(vg, stroke_width); if (stroke_color) nvgStrokeColor(vg, *stroke_color); - else nvgStrokeColor(vg, nvgRGB(0, 0, 0)); - nvgStroke(vg); + else nvgStrokeColor(vg, nvgRGB(0, 0, 0)); + nvgStroke(vg); } } @@ -817,7 +817,7 @@ class PathEndDrawer : ModelDrawer { ui_draw_line2(s, px, py, 7, &pcolor, nullptr, 3.0f); } if (isLeadDetected()) { - NVGcolor radar_stroke = COLOR_BLUE; + NVGcolor radar_stroke = COLOR_BLUE; if (lead_two_status > 0) { radar_stroke = COLOR_OCHRE; int path_width2 = lead_two_xr - lead_two_xl; @@ -1214,7 +1214,7 @@ class TurnInfoDrawer : ModelDrawer { return -1; } const auto carrot_man = sm["carrotMan"].getCarrotMan(); - + active_carrot = carrot_man.getActiveCarrot(); if (active_carrot > 1) { @@ -1238,7 +1238,7 @@ class TurnInfoDrawer : ModelDrawer { szSdiDescr = QString::fromStdString(carrot_man.getSzSdiDescr()); szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName()); szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText()); - + } else { //xTurnInfo = -1; @@ -2028,7 +2028,7 @@ class DrawCarrot : public QObject { int max_z = lane_lines[2].getZ().size(); float z_offset = 0.0; foreach(const QString & pair, pairs) { - QStringList xy = pair.split(","); // ","로 x와 y 구분 + QStringList xy = pair.split(","); // ","로 x와 y 구분 if (xy.size() == 3) { //printf("coords = x: %.1f, y: %.1f, d:%.1f\n", xy[0].toFloat(), xy[1].toFloat(), xy[2].toFloat()); float x = xy[0].toFloat(); @@ -2301,7 +2301,7 @@ class DrawCarrot : public QObject { void drawHud(UIState* s) { int show_device_state = params.getInt("ShowDeviceState"); blink_timer = (blink_timer + 1) % 16; - disp_timer = (disp_timer + 1) % 64; + disp_timer = (disp_timer + 1) % 64; nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); int x = 140;// 120; @@ -2431,6 +2431,51 @@ class DrawCarrot : public QObject { dy = by + 60; //const SubMaster& sm = *(s->sm); auto carState = sm["carState"].getCarState(); + + // Draw steering angle at top left + float steering_angle = carState.getSteeringAngleDeg(); + float steering_torque = carState.getSteeringTorqueEps(); + bool steer_fault = carState.getSteerFaultTemporary(); + + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_TOP); + + char steer_str[64]; + sprintf(steer_str, "Steer: %.1f°", steering_angle); + ui_draw_text(s, 20, 20, steer_str, 50, COLOR_WHITE, BOLD, 2.0f, 0.0f); + + // Draw canstepper status + char canstepper_str[64]; + sprintf(canstepper_str, "CanStepper: %s", steer_fault ? "FAULT" : "OK"); + NVGcolor canstepper_color = steer_fault ? COLOR_RED : COLOR_GREEN; + ui_draw_text(s, 20, 75, canstepper_str, 40, canstepper_color, BOLD, 2.0f, 0.0f); + + // Draw stepper motor torque + char torque_str[64]; + sprintf(torque_str, "Motor Torque: %.2f Nm", steering_torque); + ui_draw_text(s, 20, 120, torque_str, 35, COLOR_WHITE, BOLD, 2.0f, 0.0f); + + // Draw stepper servo status bits + int control_status_bits = (int)carState.getSteerFaultTemporary() ? 1 : 0; + char status_bits_str[128]; + sprintf(status_bits_str, "Status: TorqueCtrl=%d FeedBack=%d SoftOff=%d CloseLoop=%d", + (control_status_bits >> 0) & 1, + (control_status_bits >> 1) & 1, + (control_status_bits >> 2) & 1, + (control_status_bits >> 3) & 1); + ui_draw_text(s, 20, 160, status_bits_str, 30, COLOR_WHITE, BOLD, 2.0f, 0.0f); + + // Draw commanded steering angle + if (sm.alive("carControl")) { + auto car_control = sm["carControl"].getCarControl(); + auto actuators = car_control.getActuators(); + float commanded_steer = actuators.getSteeringAngleDeg(); + char cmd_steer_str[64]; + sprintf(cmd_steer_str, "Cmd Steer: %.1f°", commanded_steer); + ui_draw_text(s, 20, 200, cmd_steer_str, 35, COLOR_YELLOW, BOLD, 2.0f, 0.0f); + } + + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); + if (carState.getGearShifter() == cereal::CarState::GearShifter::UNKNOWN) strcpy(gear_str, "U"); else if (carState.getGearShifter() == cereal::CarState::GearShifter::PARK) strcpy(gear_str, "P"); else if (carState.getGearShifter() == cereal::CarState::GearShifter::DRIVE) { @@ -2849,7 +2894,7 @@ void ui_draw(UIState *s, ModelRenderer* model_renderer, int w, int h) { int path_x = drawPathEnd.getPathX(); int path_y = drawPathEnd.getPathY(); drawDesire.draw(s, path_x, path_y - 135); - + drawPlot.draw(s); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 40d838657f..f5f59bd4aa 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -784,6 +784,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { all_items.append(get_list((QString::fromStdString(Params().getParamPath()) + "/SupportedCars_gm").toStdString().c_str())); all_items.append(get_list((QString::fromStdString(Params().getParamPath()) + "/SupportedCars_toyota").toStdString().c_str())); all_items.append(get_list((QString::fromStdString(Params().getParamPath()) + "/SupportedCars_mazda").toStdString().c_str())); + all_items.append(get_list((QString::fromStdString(Params().getParamPath()) + "/SupportedCars_bmw").toStdString().c_str())); QMap car_groups; for (const QString& car : all_items) { diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index 4cfa3d0e3c..654bcbc193 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -17,12 +17,16 @@ void HudRenderer::updateState(const UIState &s) { is_cruise_set = false; set_speed = SET_SPEED_NA; speed = 0.0; + steering_angle = 0.0; return; } const auto &controls_state = sm["controlsState"].getControlsState(); const auto &car_state = sm["carState"].getCarState(); + // Get steering angle + steering_angle = car_state.getSteeringAngleDeg(); + // Handle older routes where vCruiseCluster is not set set_speed = car_state.getVCruiseCluster() == 0.0 ? controls_state.getVCruiseDEPRECATED() : car_state.getVCruiseCluster(); is_cruise_set = set_speed > 0 && set_speed != SET_SPEED_NA; @@ -47,6 +51,11 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) { bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0)); p.fillRect(0, 0, surface_rect.width(), UI_HEADER_HEIGHT, bg); + // Draw steering angle on top left + p.setFont(InterFont(50, QFont::Bold)); + p.setPen(QColor(255, 255, 255, 255)); + QString steerStr = QString("Steer: %1°").arg(QString::number(steering_angle, 'f', 1)); + p.drawText(QRect(20, 20, 400, 80), Qt::AlignLeft | Qt::AlignVCenter, steerStr); if (is_cruise_available) { drawSetSpeed(p, surface_rect); diff --git a/selfdrive/ui/qt/onroad/hud.h b/selfdrive/ui/qt/onroad/hud.h index b2ac379dbe..1dd261c60b 100644 --- a/selfdrive/ui/qt/onroad/hud.h +++ b/selfdrive/ui/qt/onroad/hud.h @@ -18,6 +18,7 @@ class HudRenderer : public QObject { float speed = 0; float set_speed = 0; + float steering_angle = 0; bool is_cruise_set = false; bool is_cruise_available = true; bool is_metric = false; diff --git a/system/hardware/fan_controller.py b/system/hardware/fan_controller.py index 7d5bec0509..03d63146a7 100755 --- a/system/hardware/fan_controller.py +++ b/system/hardware/fan_controller.py @@ -18,20 +18,18 @@ def __init__(self) -> None: cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_HW)) def update(self, cur_temp: float, ignition: bool) -> int: - self.controller.neg_limit = -(100 if ignition else 30) - self.controller.pos_limit = -(30 if ignition else 0) - - if ignition != self.last_ignition: - self.controller.reset() - - error = 75 - cur_temp - fan_pwr_out = -int(self.controller.update( - error=error, - feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100]) - )) + if cur_temp < 70.0: + fan_pwr_out = 0 + elif cur_temp > 85.0: + fan_pwr_out = 100 + else: + # 70°C → 20%, 85°C → 80%, target 75°C + fan_pwr_out = int(np.interp(cur_temp, [70.0, 85.0], [20, 80])) + + if not ignition: + fan_pwr_out = min(fan_pwr_out, 30) self.last_ignition = ignition return fan_pwr_out diff --git a/system/manager/manager.py b/system/manager/manager.py index b4c6f0f225..2de1f02c18 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -49,6 +49,9 @@ def manager_init() -> None: if params.get_bool("RecordFrontLock"): params.put_bool("RecordFront", True) + # Force BMW car selection + params.put("CarSelected3", "BMW E90 2005-11") + # set unset params to their default value for k in params.all_keys(): default_value = params.get_default_value(k) @@ -200,6 +203,7 @@ def main() -> None: os.system(f"python ../../opendbc/car/gm/values.py > {Params().get_param_path()}/SupportedCars_gm") os.system(f"python ../../opendbc/car/toyota/values.py > {Params().get_param_path()}/SupportedCars_toyota") os.system(f"python ../../opendbc/car/mazda/values.py > {Params().get_param_path()}/SupportedCars_mazda") + os.system(f"python ../../selfdrive/car/bmw/values.py > {Params().get_param_path()}/SupportedCars_bmw") if os.getenv("PREPAREONLY") is not None: return