Wmi to o-wmi

common/params_keys.h

@@ -144,6 +144,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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'))\"")

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

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

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