101 lines
4.0 KiB
Python
101 lines
4.0 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
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
|
|
|
|
MAX_SET_SPEED = 85 * CV.MPH_TO_MS
|
|
MIN_SET_SPEED = 20 * CV.MPH_TO_MS
|
|
|
|
|
|
class CarStateExt:
|
|
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 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]
|
|
|
|
prev_increase_button = self.increase_button
|
|
prev_decrease_button = self.decrease_button
|
|
|
|
if self.CP.openpilotLongitudinalControl:
|
|
# distance scroll wheel
|
|
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
|
|
|
|
# button logic for set-speed
|
|
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
|
|
long_press_step = 10.0 if metric else 5.0
|
|
set_speed_converted = self.set_speed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)
|
|
|
|
if self.increase_button:
|
|
if self.increase_counter % 66 == 0:
|
|
self.set_speed = (int(math.ceil((set_speed_converted + 1) / long_press_step)) * long_press_step) * conversion
|
|
elif not prev_increase_button:
|
|
self.set_speed += conversion
|
|
|
|
if self.decrease_button:
|
|
if self.decrease_counter % 66 == 0:
|
|
self.set_speed = (int(math.floor((set_speed_converted - 1) / long_press_step)) * long_press_step) * conversion
|
|
elif not prev_decrease_button:
|
|
self.set_speed -= conversion
|
|
|
|
if not ret.cruiseState.enabled:
|
|
self.set_speed = ret.vEgoCluster
|
|
|
|
# VDM_UserAdasRequest: 0=IDLE, 1=UP_1, 2=UP_2, 3=DOWN_1, 4=DOWN_2
|
|
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 == 50:
|
|
# Mimic Rivian ACC: holding stalk 0.5s sets speed to current speed (never decreases)
|
|
self.set_speed = max(self.set_speed, ret.vEgoCluster)
|
|
|
|
self.set_speed = max(MIN_SET_SPEED, min(self.set_speed, MAX_SET_SPEED))
|
|
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 = {}
|
|
|
|
if CP_IQ.flags & RivianFlagsIQ.LONGITUDINAL_HARNESS_UPGRADE:
|
|
messages[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.alt], [], 5)
|
|
|
|
return messages
|