forked from IQ.Lvbs/IQ.Pilot
168 lines
6.0 KiB
Python
168 lines
6.0 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
|
|
from enum import StrEnum
|
|
|
|
from iqdbc.car import Bus, structs
|
|
from iqdbc.car.carlog import carlog
|
|
from iqdbc.can.parser import CANParser
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.lvbs.car.toyota.values import ToyotaFlagsIQ
|
|
|
|
TRAFFIC_SIGNAL_MAP = {
|
|
1: "kph",
|
|
36: "mph",
|
|
65: "No overtake",
|
|
66: "No overtake"
|
|
}
|
|
|
|
ZSS_DIFF_THRESHOLD = 4
|
|
ZSS_MAX_THRESHOLD = 10
|
|
|
|
|
|
class IQCarState:
|
|
def __init__(self, CP, CP_IQ):
|
|
self.CP = CP
|
|
self.CP_IQ = CP_IQ
|
|
|
|
self.acc_type = 1
|
|
self.zss_compute = False
|
|
self.zss_cruise_active_last = False
|
|
self.zss_angle_offset = 0.
|
|
self.zss_threshold_count = 0
|
|
|
|
"""Initialize traffic signal variables"""
|
|
self._tsgn1 = None
|
|
self._spdval1 = None
|
|
self._splsgn1 = None
|
|
self._tsgn2 = None
|
|
self._splsgn2 = None
|
|
self._tsgn3 = None
|
|
self._splsgn3 = None
|
|
self._tsgn4 = None
|
|
self._splsgn4 = None
|
|
|
|
@staticmethod
|
|
def traffic_signal_description(tsgn):
|
|
"""Get description for traffic signal code"""
|
|
desc = TRAFFIC_SIGNAL_MAP.get(int(tsgn))
|
|
return f'{tsgn}: {desc}' if desc is not None else f'{tsgn}'
|
|
|
|
def update_traffic_signals(self, cp_cam):
|
|
"""Update traffic signals with error handling"""
|
|
try:
|
|
# Add error handling for missing RSA messages
|
|
tsgn1 = cp_cam.vl.get("RSA1", {}).get('TSGN1', 0)
|
|
spdval1 = cp_cam.vl.get("RSA1", {}).get('SPDVAL1', 0)
|
|
splsgn1 = cp_cam.vl.get("RSA1", {}).get('SPLSGN1', 0)
|
|
tsgn2 = cp_cam.vl.get("RSA1", {}).get('TSGN2', 0)
|
|
splsgn2 = cp_cam.vl.get("RSA1", {}).get('SPLSGN2', 0)
|
|
tsgn3 = cp_cam.vl.get("RSA2", {}).get('TSGN3', 0)
|
|
splsgn3 = cp_cam.vl.get("RSA2", {}).get('SPLSGN3', 0)
|
|
tsgn4 = cp_cam.vl.get("RSA2", {}).get('TSGN4', 0)
|
|
splsgn4 = cp_cam.vl.get("RSA2", {}).get('SPLSGN4', 0)
|
|
except (KeyError, AttributeError) as e:
|
|
# Handle case where RSA messages are not available
|
|
carlog.debug(f"RSA messages not available: {e}")
|
|
return
|
|
|
|
has_changed = tsgn1 != self._tsgn1 \
|
|
or spdval1 != self._spdval1 \
|
|
or splsgn1 != self._splsgn1 \
|
|
or tsgn2 != self._tsgn2 \
|
|
or splsgn2 != self._splsgn2 \
|
|
or tsgn3 != self._tsgn3 \
|
|
or splsgn3 != self._splsgn3 \
|
|
or tsgn4 != self._tsgn4 \
|
|
or splsgn4 != self._splsgn4
|
|
|
|
self._tsgn1 = tsgn1
|
|
self._spdval1 = spdval1
|
|
self._splsgn1 = splsgn1
|
|
self._tsgn2 = tsgn2
|
|
self._splsgn2 = splsgn2
|
|
self._tsgn3 = tsgn3
|
|
self._splsgn3 = splsgn3
|
|
self._tsgn4 = tsgn4
|
|
self._splsgn4 = splsgn4
|
|
|
|
if not has_changed:
|
|
return
|
|
|
|
carlog.debug('---- TRAFFIC SIGNAL UPDATE -----')
|
|
if tsgn1 is not None and tsgn1 != 0:
|
|
carlog.debug(f'TSGN1: {self.traffic_signal_description(tsgn1)}')
|
|
if spdval1 is not None and spdval1 != 0:
|
|
carlog.debug(f'SPDVAL1: {spdval1}')
|
|
if splsgn1 is not None and splsgn1 != 0:
|
|
carlog.debug(f'SPLSGN1: {splsgn1}')
|
|
if tsgn2 is not None and tsgn2 != 0:
|
|
carlog.debug(f'TSGN2: {self.traffic_signal_description(tsgn2)}')
|
|
if splsgn2 is not None and splsgn2 != 0:
|
|
carlog.debug(f'SPLSGN2: {splsgn2}')
|
|
if tsgn3 is not None and tsgn3 != 0:
|
|
carlog.debug(f'TSGN3: {self.traffic_signal_description(tsgn3)}')
|
|
if splsgn3 is not None and splsgn3 != 0:
|
|
carlog.debug(f'SPLSGN3: {splsgn3}')
|
|
if tsgn4 is not None and tsgn4 != 0:
|
|
carlog.debug(f'TSGN4: {self.traffic_signal_description(tsgn4)}')
|
|
if splsgn4 is not None and splsgn4 != 0:
|
|
carlog.debug(f'SPLSGN4: {splsgn4}')
|
|
carlog.debug('------------------------')
|
|
|
|
def calculate_speed_limit(self):
|
|
"""Calculate speed limit from traffic signals with validation"""
|
|
# Check all traffic sign slots for speed limits, not just tsgn1
|
|
for tsgn, spdval in [(self._tsgn1, self._spdval1),
|
|
(self._tsgn2, None),
|
|
(self._tsgn3, None),
|
|
(self._tsgn4, None)]:
|
|
|
|
if tsgn == 1 and spdval is not None and 0 < spdval <= 200: # Reasonable speed range
|
|
return spdval * CV.KPH_TO_MS
|
|
|
|
if tsgn == 36 and spdval is not None and 0 < spdval <= 120: # Reasonable MPH range
|
|
return spdval * CV.MPH_TO_MS
|
|
|
|
return 0
|
|
|
|
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
|
cp = can_parsers[Bus.pt]
|
|
cp_cam = can_parsers[Bus.cam]
|
|
|
|
if self.CP_IQ.flags & ToyotaFlagsIQ.SMART_DSU or self.CP_IQ.flags & ToyotaFlagsIQ.STOP_AND_GO_HACK:
|
|
self.acc_type = 1
|
|
|
|
if self.CP_IQ.enableGasInterceptor:
|
|
gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
|
ret.gasPressed = gas > 805
|
|
|
|
# ZSS support thanks to zorrobyte, ErichMoraga, and dragonpilot
|
|
if self.CP_IQ.flags & ToyotaFlagsIQ.ZSS:
|
|
zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
|
|
control_available = ret.cruiseState.available
|
|
|
|
# Only compute ZSS offset when control is available
|
|
if control_available and not self.zss_cruise_active_last:
|
|
self.zss_threshold_count = 0
|
|
self.zss_compute = True # Control was just activated, so allow offset to be recomputed
|
|
self.zss_cruise_active_last = control_available
|
|
|
|
# Compute ZSS offset once we have meaningful angles
|
|
if self.zss_compute and abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
|
|
self.zss_compute = False
|
|
self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg
|
|
|
|
# Sanity checks
|
|
steering_angle_deg = zorro_steer - self.zss_angle_offset
|
|
if self.zss_threshold_count <= ZSS_MAX_THRESHOLD:
|
|
if abs(ret.steeringAngleDeg - steering_angle_deg) > ZSS_DIFF_THRESHOLD:
|
|
self.zss_threshold_count += 1
|
|
else:
|
|
ret.steeringAngleDeg = steering_angle_deg
|
|
|
|
# Update traffic signals and speed limit
|
|
self.update_traffic_signals(cp_cam)
|
|
ret_iq.speedLimit = self.calculate_speed_limit()
|