IQ.Pilot Prebuilt Release @ ab07000

iqdbc_repo/iqdbc/lvbs/car/rivian/iq_carstate.py

@@ -0,0 +1,104 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Rivian longitudinal-harness-upgrade carState reader: with the upgrade harness the
+right steering-wheel controls and the drive stalk drive the openpilot set speed,
+and the harness exposes blind-spot indicators. Only active behind the upgrade flag.
+"""
+import math
+from enum import StrEnum
+
+from iqdbc.car import Bus, structs
+from iqdbc.can.parser import CANParser
+from iqdbc.car.common.conversions import Conversions as CV
+from iqdbc.car.rivian.values import DBC
+from iqdbc.lvbs.car.rivian.values import RivianFlagsIQ
+
+ButtonType = structs.CarState.ButtonEvent.Type
+
+_SET_SPEED_MAX = 85 * CV.MPH_TO_MS
+_SET_SPEED_MIN = 20 * CV.MPH_TO_MS
+_LONG_PRESS_FRAMES = 66
+_STALK_HOLD_FRAMES = 50
+
+
+class IQCarState:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+
+    self.set_speed = 10
+    self.increase_button = False
+    self.decrease_button = False
+    self.distance_button = 0
+    self.increase_counter = 0
+    self.decrease_counter = 0
+    self.stalk_down_counter = 0
+
+  def _apply_set_speed_buttons(self, ret: structs.CarState, cp_park, cp_adas) -> None:
+    was_increasing = self.increase_button
+    was_decreasing = self.decrease_button
+
+    self.increase_button = cp_park.vl["WheelButtons"]["RightButton_RightClick"] == 2
+    self.decrease_button = cp_park.vl["WheelButtons"]["RightButton_LeftClick"] == 2
+    self.increase_counter = self.increase_counter + 1 if self.increase_button else 0
+    self.decrease_counter = self.decrease_counter + 1 if self.decrease_button else 0
+
+    metric = cp_adas.vl["Cluster"]["Cluster_Unit"] == 0
+    conversion = CV.KPH_TO_MS if metric else CV.MPH_TO_MS
+    step = 10.0 if metric else 5.0
+    shown = self.set_speed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)
+
+    # A held button steps to the next round multiple; a tap nudges by one unit.
+    if self.increase_button:
+      if self.increase_counter % _LONG_PRESS_FRAMES == 0:
+        self.set_speed = math.ceil((shown + 1) / step) * step * conversion
+      elif not was_increasing:
+        self.set_speed += conversion
+    if self.decrease_button:
+      if self.decrease_counter % _LONG_PRESS_FRAMES == 0:
+        self.set_speed = math.floor((shown - 1) / step) * step * conversion
+      elif not was_decreasing:
+        self.set_speed -= conversion
+
+  def update_longitudinal_upgrade(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
+    cp_park = can_parsers[Bus.alt]
+    cp_adas = can_parsers[Bus.adas]
+    cp = can_parsers[Bus.pt]
+
+    if self.CP.openpilotLongitudinalControl:
+      right_scroll = cp_park.vl["WheelButtons"]["RightButton_Scroll"]
+      if right_scroll != 255:
+        if self.distance_button != right_scroll:
+          ret.buttonEvents = [structs.CarState.ButtonEvent(pressed=False, type=ButtonType.gapAdjustCruise)]
+        self.distance_button = right_scroll
+
+      self._apply_set_speed_buttons(ret, cp_park, cp_adas)
+
+      if not ret.cruiseState.enabled:
+        self.set_speed = ret.vEgoCluster
+
+      # Drive stalk held down (VDM_UserAdasRequest 3/4) for ~0.5s snaps set speed
+      # up to the current speed, matching stock Rivian ACC (it never lowers it).
+      stalk_down = int(cp.vl["VDM_AdasSts"]["VDM_UserAdasRequest"]) in (3, 4)
+      self.stalk_down_counter = self.stalk_down_counter + 1 if stalk_down else 0
+      if self.stalk_down_counter == _STALK_HOLD_FRAMES:
+        self.set_speed = max(self.set_speed, ret.vEgoCluster)
+
+      self.set_speed = max(_SET_SPEED_MIN, min(self.set_speed, _SET_SPEED_MAX))
+      ret.cruiseState.speed = self.set_speed
+
+    if self.CP.enableBsm:
+      ret.leftBlindspot = cp_park.vl["BSM_BlindSpotIndicator"]["BSM_BlindSpotIndicator_Left"] != 0
+      ret.rightBlindspot = cp_park.vl["BSM_BlindSpotIndicator"]["BSM_BlindSpotIndicator_Right"] != 0
+
+  def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
+    if self.CP_IQ.flags & RivianFlagsIQ.LONGITUDINAL_HARNESS_UPGRADE:
+      self.update_longitudinal_upgrade(ret, can_parsers)
+
+  @staticmethod
+  def get_parser(CP, CP_IQ) -> dict[StrEnum, CANParser]:
+    messages: dict[StrEnum, CANParser] = {}
+    if CP_IQ.flags & RivianFlagsIQ.LONGITUDINAL_HARNESS_UPGRADE:
+      messages[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.alt], [], 5)
+    return messages